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

Abstract

【課題】移動体の位置姿勢計測において、時間が経過するにつれて、計測誤差が蓄積することで、位置姿勢計測の精度が低下することがあった。【解決手段】情報処理装置方法であって、移動体に搭載された撮像装置が撮像したシーンの三次元位置情報と該撮像装置の位置及び姿勢と、該撮像装置の位置及び姿勢の拘束条件を地図要素として含む、複数の前記地図要素から構成される三次元地図を保持し、前記拘束条件を判定するためのセンサの計測値を取得し、前記計測値に基づいて、前記拘束条件を判定し、前記拘束条件に基づいて、前記三次元地図を補正する。【選択図】 図２

Description

本発明は、三次元地図に関する情報処理装置、情報処理方法、及びコンピュータプログラム等に関する

移動ロボットや無人搬送車（ＡＧＶ：Ａｕｔｏｎｏｍｏｕｓ Ｇｕｉｄｅｄ Ｖｅｈｉｃｌｅ）等の移動体に搭載した、カメラやＬｉＤＡＲ（ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）等の計測機器で計測した画像情報や距離情報等の計測情報に基づいて、自律移動する移動体システムの開発が進められている。又、現実空間に対して高精度に移動体の位置姿勢を推定する技術が求められている。特許文献１では、カメラで撮影した画像中の特徴点と移動体の拘束条件である運動モデルとに基づいて、移動体の位置姿勢の補正量を算出している。又、非特許文献１には、カメラの位置姿勢を算出する方法が記載されている。

特開２０２１－１１３７６０号公報

Ｒａｕｌ Ｍｕｒ－Ａｒｔａｌ ｅｔ．ａｌ，ＯＲＢ－ＳＬＡＭ：Ａ Ｖｅｒｓａｔｉｌｅ ａｎｄ Ａｃｃｕｒａｔｅ Ｍｏｎｏｃｕｌａｒ ＳＬＡＭ Ｓｙｓｔｅｍ．ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓｏｎ Ｒｏｂｏｔｉｃｓ

［課題］
しかし特許文献１や非特許文献１の方法では、時間の経過につれて誤差が累積するため、位置姿勢計測の精度が低下する問題があった。

［目的］
本発明は以上を鑑みて、位置姿勢計測の精度を向上した情報処理装置を提供することを目的とする。

本発明の１つの側面の情報処理装置は、
移動体に搭載された撮像装置が撮像したシーンの三次元位置情報と、前記撮像装置の位置及び姿勢と、前記撮像装置の位置及び姿勢の拘束条件を含む、複数の地図要素から構成される三次元地図を保持する地図保持手段と、
前記拘束条件を判定するためのセンサの計測値を取得するセンサ計測値取得手段と、
前記センサ計測値取得手段が取得した計測値に基づいて、前記拘束条件を判定する拘束条件判定手段と、
前記拘束条件判定手段が判定した前記拘束条件に基づいて、前記三次元地図を補正する地図補正手段と、を備えることを特徴とする。

本発明によれば、位置姿勢計測の精度を向上した情報処理装置を実現することができる。

実施形態１に係る装置の利用形態を説明する図である。 実施形態１に係る情報処理装置１００を含む移動体１（ＡＧＶ）の構成例を示す機能ブロック図である。 情報処理装置１００のハードウェア構成例を示す図である。 実施形態１に係る移動体１（ＡＧＶ）全体の処理の流れを示すフローチャートである。 図４のステップＳ１０７の処理の流れを示すフローチャートである。 実施形態２に係る情報処理装置１００を含む移動体１の機能ブロック図である。

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

（実施形態１）
実施形態１について、水平に施工された床面上を移動するＡＧＶに適用する事例を用いて説明する。本実施形態のＡＧＶシステムでは、公知のＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）技術により、ＡＧＶに搭載したカメラの位置姿勢の計測と、位置姿勢を計測するための地図情報の生成を行う。

ここで地図情報は、カメラで撮影した画像から抽出した特徴点群の三次元座標を含むが、一般に、ＳＬＡＭで生成した地図情報に含まれる位置情報には誤差が蓄積するため、地図の形状がＡＧＶの物理的な移動軌跡と異なるという問題がある。

本実施形態では、ＡＧＶに固定した２軸角度計の出力から算出したＡＧＶの姿勢が水平であれば、その地点でのＡＧＶの姿勢が水平となるように制約を課して地図要素の位置を補正する。この方法により、ＡＧＶの物理的な移動軌跡である水平面に従って、地図の形状が修正されるため、当該地図に基づくカメラ位置姿勢の計測精度が向上する。

ここで、地図要素は、カメラの位置姿勢、特徴点群の三次元座標に加え、カメラの位置姿勢の計測時刻と同期する２軸角度計の計測値と、制約を表す拘束情報等を保持する。

図１は実施形態１に係る装置の利用形態を説明する図である。利用者２００は、コントローラ３００を利用して、情報処理装置１００を搭載した移動体１（ＡＧＶ）の移動、回転等を制御し、自律移動させる予定の経路上を移動させる。

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

又、図２に示される夫々の機能ブロックは、同じ筐体に内蔵されていなくても良く、互いに信号路を介して接続された別々の装置により構成しても良い。

撮像装置１０はＣＭＯＳイメージセンサ等を含むカメラであり、撮像情報入力部１１（入力手段）に撮影した画像を出力する。又、移動体１が水平に配置されたときにカメラの光軸が地面に対して水平になるように設置されており、ヨー軸が鉛直方向と一致し、ロール軸がカメラの光軸方向と一致し、ピッチ軸はヨー軸とロール軸の外積で表されるものとする。撮像情報入力部１１は、カメラと接続されており、カメラが撮像した画像を時系列（例えば、毎秒６０フレーム）で入力し、位置姿勢算出部１２に出力する。

位置姿勢算出部１２（位置姿勢算出手段）は、地図保持部１１０が保持する地図情報と撮像情報入力部１１から入力された画像から検出（抽出）した特徴点の情報を用いて例えばパターンマッチングによりカメラの位置姿勢を算出する。又、算出した位置姿勢を地図要素追加部１３に出力する。

センサ１４は例えば２軸角度計を含み、カメラのロール軸周りの回転量とピッチ軸周りの回転量を検出し出力することによりカメラの傾斜角度を計測する。即ち、２軸角度計を用いてカメラの重力軸方向に対する傾斜角度を計測して水平方向を移動しているかどうかを検知する。センサ１４の出力は拘束条件を判定するために用いられる。

尚、カメラの撮影と２軸角度計の計測は同じタイミングで行われるものとする。センサ情報入力部１５は、２軸角度計と接続されており、２軸角度計の計測値を入力し、地図要素追加部１３（地図要素追加手段）に出力する。

地図要素追加部１３は、撮像されたシーンの三次元位置情報や位置姿勢算出部１２が算出したカメラの位置姿勢や、センサ情報入力部１５が出力した計測値や後述の拘束条件等に基づき新たな地図要素を作成し、地図保持部１１０に出力する。

移動指令取得部１６は、コントローラ３００が出力した結果を入力として、移動制御情報に変換して、移動制御部１７に出力する。移動制御部１７は、移動指令取得部１６が出力した結果を入力として、移動体１の駆動や移動方向等を制御する。

地図保持部１１０は地図保持手段として機能しており、地図情報を保持する地図保持ステップを実行する。具体的には、地図保持部１１０は、移動体に搭載された撮像装置が撮像したシーンの三次元位置情報と、撮像装置の位置及び姿勢と、撮像装置の位置及び姿勢の拘束条件とを含む、複数の地図要素から構成される三次元地図を保持する。

地図保持部１１０は、保持する地図情報、又は地図情報に含まれる地図要素の情報を必要に応じて位置姿勢算出部１２に出力する。又、地図要素追加部１３が追加した地図要素の情報や、地図補正部１１４が出力した地図要素の情報等を入力し、保持する。

センサ計測値取得部１１５（センサ計測値取得手段）は、地図保持部１１０が保持する最新の地図要素や、当該地図要素が保持するセンサ計測値を取得し、拘束条件判定部１１１に出力する。

拘束条件判定部１１１（拘束条件判定手段）は、センサ計測値取得部１１５が出力したセンサ計測値等に基づき撮像装置の位置姿勢の拘束条件が存在するかどうかを判定し、判定した結果を拘束情報生成部１１２に出力する。

拘束情報生成部１１２は、拘束条件判定部１１１が出力した判定結果に基づいて、拘束条件の設定に必要な情報（カメラのロール軸周りの回転量、ピッチ軸周りの回転量、ロール軸、ピッチ軸周りの回転量を補正時に固定するか否かの情報）を生成する。そして、地図補正拘束条件設定部１１３に出力する。

地図補正拘束条件設定部１１３は、地図保持部１１０が保持する最新の地図要素を取得し、拘束情報生成部１１２が生成した拘束情報を地図要素に設定し、地図保持部１１０に出力する。

地図補正部１１４（地図補正手段）は、地図保持部１１０から複数の地図要素を取得する。そして、拘束条件判定部１１１で判定され地図補正拘束条件設定部１１３で設定された拘束条件に基づいて、地図要素に含まれる撮像装置の位置姿勢や特徴点群の三次元座標を補正する。又、補正した結果を地図保持部１１０に出力する。

図３は、情報処理装置１００のハードウェア構成例を示す図である。３１はＣＰＵであり、システムバス３０に接続された各種デバイスの制御及び情報処理装置１００の各種処理を行う。３２はＲＯＭであり、ＢＩＯＳのプログラムやブートプログラムを記憶する。３３はＲＡＭであり、ＣＰＵである３１の主記憶装置として使用される。３４は外部メモリであり、情報処理装置１００が処理するコンピュータプログラムを格納する。入力部３５はキーボードやマウスなど情報等の入力に係る処理を行う。

表示部３６は３１からの指示に従って情報処理装置１００の演算結果を外部の表示装置に出力するための表示制御手段として機能している。尚、表示装置は液晶表示装置やプロジェクタ、ＬＥＤインジケーターなど、種類は問わない。又、表示装置はコントローラ３００の表示装置であっても良い。３７は通信インターフェイスであり、ネットワークを介して情報通信を行うものであり、通信インターフェイスはイーサネットでもよく、ＵＳＢやシリアル通信、無線通信等種類は問わない。

尚、位置姿勢算出部１２とは、通信インターフェイス３７を介して、算出した位置姿勢情報のやり取りを行う。３８はＩ／Ｏであり、撮像装置１０から画像を入力する。又、３３は後述する地図補正部が実施する補正で取得する地図要素の数や、２軸角度計の計測値から水平であるかどうかを判定するための閾値等の処理ステップ上必要な固定パラメータを保持する。

図４は、実施形態１に係る移動体１（ＡＧＶ）全体の処理の流れを示すフローチャートであり、図５は図４のステップＳ１０７の処理の流れを示すフローチャートである。尚、情報処理装置内のコンピュータ３１がメモリに記憶されたコンピュータプログラムを実行することによって図４、図５のフローチャートの各ステップの動作が行われる。
図４のフローは、例えば移動体の走行を開始させる指示が行われるのに伴って開始する。

ステップＳ１０１では、情報処理装置１００は、システムの初期化を行う。即ち、プログラムを読込み、情報処理装置１００を動作可能な状態にする。又、地図情報やカメラパラメータを読み込む。ステップＳ１０２では、撮像情報入力部１１が、カメラが撮像した画像を入力する。

ステップＳ１０３では、位置姿勢算出部１２が、ステップＳ１０２で入力した画像と、地図保持部１１０が保持する地図情報から例えばパターンマッチングや非特許文献１に記載された方法で、カメラの位置姿勢を算出する。算出を終えたらステップＳ１０４に進む。

ステップＳ１０４では、センサ情報入力部１５が、ステップＳ１０２のカメラの撮影画像の撮像と同期してセンサ１４から取得された２軸角度計の計測値を入力し、ステップＳ１０５に進む。ここで、ステップＳ１０４は、センサの計測値を取得するセンサ計測値取得ステップとして機能している。

ステップＳ１０５では、地図要素追加部１３が、地図要素の追加の必要の有無を判定する。地図要素追加の判定方法は非特許文献１に記載の方法を用いても良い。地図要素の追加が必要であれば、ステップＳ１０６に進む。必要でなければ、ステップＳ１０８に進む。

ステップＳ１０６では、地図要素追加部１３が、新たな地図要素を作成し、地図保持部１１０が保持する地図情報に地図要素を追加する。地図要素の追加は、非特許文献１に記載の方法を用いても良い。ステップＳ１０４で入力した２軸角度計の計測値も併せて地図要素に登録する。地図追加が完了したらステップＳ１０７に進む。

ステップＳ１０７では、情報処理装置１００がステップＳ１０６で追加した地図要素に対する拘束条件を追加する。本ステップの処理の詳細については後述する。拘束条件の追加が完了すれば、ステップＳ１０８に進む。

ステップＳ１０８（地図補正ステップ）では、地図補正部１１４が、ステップＳ１０６が追加した最新の地図要素と、地図保持部が保持する過去に取得した地図要素のうち新しい順の所定数の地図要素と、を取得する。取得した地図要素を対象に、ステップＳ１０７で設定した拘束情報に基づいて、複数の地図要素に含まれる位置姿勢と特徴点群の三次元座標を追加又は補正し三次元地図を補正する。

具体的には、地図要素が拘束情報を保持している場合、拘束情報に含まれるカメラのロール軸及びピッチ軸周りの回転量と地図要素に含まれる位置姿勢のロール軸及びピッチ軸周りの回転量の差分量を算出する。算出した差分量を用いて、地図要素に含まれる位置姿勢のロール軸及びピッチ軸周りの回転量が、拘束情報に含まれるロール軸及びピッチ軸周りの回転量の値に一致するように位置姿勢を補正する。

このように、センサ１４の出力に基づき生成された拘束情報により地図要素内の回転量などに制限を設定したり、回転量をリセットしたりすることで地図の歪みや変形を補正する。

取得したすべての地図要素を対象に、ステップＳ１０７で設定したカメラのロール軸及びピッチ軸周りの回転量の変化量を０にする拘束条件を含めて、非特許文献１に記載のローカルバンドル調整を実施する。そして、地図要素に含まれる位置姿勢と特徴点群の三次元座標を補正する。補正が完了したらステップＳ１０９に進む

ステップＳ１０９では、システムを終了するか否かの判断を行う。本実施形態においては、移動体が目的地に到着した場合にコントローラ３００がシステムを終了するコマンドを入力したか否か判定する。終了コマンドが入力された場合にはシステムを終了し、そうでなければステップＳ１０２に戻り、処理を継続する。

図５は、図４のステップＳ１０７の処理の流れを示すフローチャートである。ステップ２０１で、センサ計測値取得部１１５が、ステップＳ１０４が入力した地図要素に含まれる位置姿勢と同時刻の２軸角度計が計測したカメラのロール軸周りの回転量とピッチ軸周りの回転量を入力し、ステップＳ２０２に進む。

ステップＳ２０２（拘束条件判定ステップ）では、拘束条件判定部１１１が、ステップＳ２０１で入力したカメラのロール軸周りの回転量が所定の値以下かつピッチ軸周りの回転量が所定の値以下かという判定をする。判定結果がＹｅｓであれば判定結果を１（水平である）と設定し、判定結果がＮＯであれば０（水平でない）と設定する。即ち、ステップＳ２０２は、センサ計測値取得手段が取得した計測値に基づいて、拘束条件を判定する。

ステップＳ２０３では、判定結果が１か否かを判定し、ステップＳ２０２の判定結果が１（水平である）であれば、ステップＳ２０４に進み、０（水平でない）であれば、ステップＳ２０６に進む。

ステップＳ２０４では、拘束情報生成部１１２が、ステップＳ２０１で入力したカメラのロール軸周りの回転量とピッチ軸周りの回転量、ロール軸及びピッチ軸周りの回転量の地図補正時の変化量を０にするフラグを含む拘束情報データセットを生成する。そして、ステップＳ２０５に進む。

ステップＳ２０５では、地図補正拘束条件設定部１１３が、ステップＳ２０４で生成した拘束情報を表すデータセットを、ステップＳ１０６で追加した地図要素に追加する。即ち、ステップＳ２０５において、地図要素追加手段が追加した地図要素の拘束条件を設定し、図５のフローを終了する。

以上のように、実施形態１では、撮像装置の傾斜角度を計測するセンサ１４の計測値に基づいて、地図補正時のキーフレームの位置姿勢に拘束条件を設定し、その拘束条件を満たすように地図の補正処理を実施する。これにより、ＡＧＶが水平面を移動しているにもかかわらず、地図が水平に対して歪んで計測される現象を抑制することができる。

又、高精度な姿勢情報を拘束条件に設定することにより、地図補正時の初期位置の精度が向上し、すべての補正対象のパラメータの補正の精度が向上する。その結果、地図の精度が向上し、地図に基づく位置姿勢計測の精度が向上する。更に、地図補正処理時に変動させるパラメータ数が減ること、初期位置の精度が向上することにより、地図補正に必要な処理時間を短縮することもできる。

実施形態１では、センサ１４は２軸角度計を用いて撮像装置の重力軸方向に対する傾斜角度を計測し、センサ情報入力部は、センサ１４の計測値に基づき撮像装置が水平面上を移動しているか、或いは水平方向を移動しているかどうかを判定した。しかし撮像装置と重力軸方向の傾斜角度を推定可能なセンサを用いればよく、慣性センサ（ＩＭＵ：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）を用いて推定しても良い。

具体的には、Ｔｏｎｇ Ｑｉｎｅｔらの方法（Ｔｏｎｇ Ｑｉｎｅｔ．ａｌ ＶＩＮＳ－Ｍｏｎｏ：Ａ Ｒｏｂｕｓｔ ａｎｄ Ｖｅｒｓａｔｉｌｅ Ｍｏｎｏｃｕｌａｒ Ｖｉｓｕａｌ－Ｉｎｅｒｔｉａｌ Ｓｔａｔｅ Ｅｓｔｉｍａｔｏｒ）の手法を使っても良い。即ち、Ｖｉｓｕａｌ－ＳＬＡＭの計測結果とＩＭＵの計測結果の整合性が取れるように初期化を行い、重力軸の推定を行っても良い。或いは、ＳＬＡＭでの撮像装置の位置姿勢の推定結果のヨー軸と重力軸とがなす角（傾斜角度）に基づいて水平であるかどうかの判定を行っても良い。

尚、実施形態１では撮像装置と重力軸方向の傾斜角度から水平であるかどうかを判定したが、水平に限らず、センサ計測値である傾斜角度で規定される平面を走行しているという拘束にしても良い。

具体的には、撮像装置の位置姿勢のうち、ロール軸周りの回転量とピッチ軸周りの回転量の２つのパラメータは、センサ計測値から得られる値を真値とする。そして、地図を補正する際、この２つのパラメータが、常にセンサ計測値の値に整合するように固定し補正しても良い。

又、センサ１４として２軸角度計を用いる代わりに、撮像装置との相対位置姿勢が既知のＴｏＦ（Ｔｉｍｅ ｏｆ Ｆｌｉｇｈｔ）やＬｉＤＡＲ、ステレオカメラなどの距離計測可能なセンサを用いても良い。即ち、これらのセンサで床面を計測して得られる三次元点群から主成分分析を行い、平面を推定し、推定した平面の法線方向から撮像装置のヨー軸の方向を算出し、算出した値を真値とする。

更に、地図を補正する際には、算出したヨー軸と地図要素に含まれる撮像装置のヨー軸からロール軸及びピッチ軸周りの回転量の差分量を計算する。そして撮像装置のロール軸及びピッチ軸周りの回転量に上記差分量を足し合わせた値で、撮像装置のロール軸及びピッチ軸周りの回転量を固定して、補正しても良い。

尚、ここでは床面を計測する例を説明したが、壁面や天井面の向きを計測して間接的に移動する面の傾きを推定しても良い。例えば、壁面に対して垂直な面を移動しているときの壁面の法線を算出し、壁面の法線方向から撮像装置のヨー軸の方向を算出し、このときの撮像装置の姿勢を基準姿勢とする。

そして、壁面の法線方向から撮像装置のヨー軸の方向を算出し、算出したヨー軸と基準姿勢のヨー軸から、基準姿勢に対するロール軸及びピッチ軸周りの回転量を算出し、撮像装置のロール軸及びピッチ軸周りの回転量を算出した値で固定して補正しても良い。

又、水平の目印として、窓枠や水平に並んだ光学マーカなどを認識して、水平のエッジが画像上で水平に映っているかどうかで水平であるかを判定しても良い。例えば、水平に移動体を配置したときに、水平に並んだ光学マーカを認識したときの水平のエッジの角度とその時の姿勢を基準姿勢として保持しておく。

そして、保持している角度との差分が所定量以上になった場合、水平でないと判定し、所定量未満の場合、水平であると判定する。水平であれば、基準姿勢のロール軸及びピッチ軸周りの回転量を正解値として固定して、補正しても良い。

実施形態１では、拘束条件の設定は水平であるか否かの二値で判定していたが、撮像装置が水平面上を移動している度合いや撮像装置の水平方向を移動している度合いや床の平面性の度合い等に応じて拘束条件の重みを変更しても良い。例えば、地図要素を追加してから次の地図要素を追加するまでの間の傾斜角度の分散から拘束の重みを設定しても良い。そして、前者の場合、傾斜角度の分散が大きいほど拘束の重みを小さくし、分散が小さいほど拘束の重みを大きくすれば良い。

或いは予め傾斜角度に応じて重みを決めた表を用意しておいて傾斜角度に対応した重みを設定しても良い。そして、傾斜角度が大きいほど拘束の重みを小さくし、傾斜角度が小さいほど拘束の重みを大きくすれば良い。

又、床面を距離計測可能なセンサで計測する場合は、推定した平面と推定に用いた三次元点群との距離の分散の大きさを拘束の重みに設定しても良い。ＡＧＶなどの車輪で走行する移動ロボットは、平面上を走行する頻度が高いが、整地された平面を走行することもあれば、多少凹凸があるが平面に近似可能な面上を走行することもある。

拘束に重みを設定することで、整地された平面は拘束の重みを大きくして、凹凸のある平面は重みを小さくするといった設定が可能となり、より環境に適応した拘束条件を設定することが可能となる。

実施形態１ではローカルバンドル調整を用いて地図を補正していたが、拘束条件を設定して補正可能な方法であれば良く、ポーズグラフ最適化を用いて地図の補正を行っても良い。これらの補正は、非特許文献１に記載された方法を用いても良い。

実施形態１では、水平である又は平面であるかを計測するためのセンサの計測値を利用して拘束条件を判定していたが、ＡＧＶなどの移動ロボットの場合、移動に用いる制御情報があるので、それらの制御入力を拘束条件に設定しても良い。

具体的には、ＡＧＶの移動で利用する制御情報を計測するセンサの計測値に基づいて拘束条件を判定する。例えば、ＡＧＶが直進するという制御情報が複数の地図要素間で継続して計測されているならば、その複数の地図要素間のピッチ軸方向（横方向）のパラメータの変化量を０にすることができる。

又、ＡＧＶがその場で旋回をするという制御情報が複数の地図要素間で継続して計測されている場合には、ＡＧＶの位置は同一円周上にあるという拘束条件を設定しても良い。これにより、実世界で直進動作をしているにもかかわらず、地図情報が歪むような現象を抑制することができる。このように、センサは、移動体の制御情報を計測するものであってもよく、その計測値に基づいて拘束条件の重みを設定するようにしても良い。

実施形態１では、水平であるかどうかを拘束条件として判定していたが、撮像装置の周辺の物体の配置情報から拘束条件を判定しても良い。例えば撮像装置との相対位置姿勢が既知のＴｏＦやＬｉＤＡＲ、ステレオカメラなどの距離を計測可能なセンサを用いて、撮像装置と周囲の物体の距離を計測し、その距離の計測値からＡＧＶの位置姿勢の拘束条件を判定しても良い。

即ち、ある地点で周囲の物体との距離を計測し、その地点から先１００ｍの間は左右に壁があり、横幅が１０ｍであると計測し、撮像装置が横幅１０［ｍ］に対して中心に位置しているとする。その場合には、その地点からの横方向の並進移動成分の変化の範囲を±５［ｍ］にする拘束条件を設定する。

又は、ある地点で天井との距離を計測し、その地点から先５０ｍの天井の高さが撮像装置より５［ｍ］高い位置にあるならば、その地点からの上方向の並進移動成分の変化の範囲を０［ｍ］から５［ｍ］の間に制限するような拘束条件を設定する。

このような位置に関する制約を加えることにより、実世界での走行経路から地図が逸脱することを防ぐことができる。又、位置の拘束条件に反映させる物体と反映させない物体を画像に基づく機械学習による物体認識（物体解析）によって判定しても良い。例えば、壁などは物理的に動かないので拘束条件に反映させ、台車などは移動する可能性があるため拘束条件に反映させないようにしても良い。

即ち、センサ計測値取得部１１５が取得した計測値に基づいて、物体認識手段が認識した壁などの物体までの距離を算出し、算出された距離に基づいて拘束条件を設定しても良い。

実施形態１では、水平かどうかのみを拘束条件として判定していたが、必ずしも１つの拘束条件である必要はなく、複数の拘束条件を組み合わせることにより最終的な撮像装置の拘束条件を設定しても良い。例えば、水平の拘束によりロール軸周りの回転量とピッチ軸周りの回転量のパラメータの変化量を０にする。そして、周辺の物体の配置情報の拘束により撮像装置の位置が横幅１０ｍの区間から逸脱することがないように撮像装置の並進移動成分のパラメータに拘束条件を設けても良い。

他には、平面の拘束によりロール軸周りの回転量とピッチ軸周りの回転量のパラメータの変化量を０にし、移動の制御入力の直進の拘束によりピッチ軸方向（横方向）のパラメータの変化量を０にするという拘束条件を設ける。このように複数の拘束条件を組み合わせても良い。

即ち、複数のセンサの計測値に基づいて複数の拘束条件を設定し、複数の拘束条件の組み合わせに基づき三次元地図を補正することで変動するパラメータの数を制限することができるため、地図を補正する際に補正の精度を高められる。又、処理時間を短縮することができる。

尚、カメラはモノクロ画像を取得するものであっても良いし、カラー画像を用いても良い。又、カメラが地面に対して水平に取り付けられていることを前提にしていたが、カメラの水平方向に対する傾斜角度を事前に計測してあれば、必ずしも水平に設置する必要はない。

具体的には、センサが水平を検出したときのカメラの傾斜角度を事前に計測しておく。そして、センサが水平であると判定したときに、地図要素のカメラの位置姿勢のロール軸周り、ピッチ軸周りの回転量と、事前に計測したカメラのロール軸周り、ピッチ軸周りの回転量の差分量を夫々算出する。算出した差分量を用いて、地図要素に含まれるカメラ位置姿勢のロール軸周り、ピッチ軸周りの回転量が、事前に計測した水平時のカメラのロール軸周り、ピッチ軸周りの回転量と一致するように位置姿勢を補正すれば良い。

実施形態１では、情報処理装置１００は移動体に搭載される例を説明したが、情報処理装置１００はネットワークを介して接続されたサーバー上に設けることもできる。又、地図保持部１１０のみ外部のサーバー上に設けることも可能である。

尚、実施形態１では、カメラの撮像タイミングと２軸角度計の計測タイミングを同期させることを前提としていたが、必ずしも計測タイミングを同期する必要はない。例えば、２軸角度計の計測値を常に保持しておいて、カメラの撮像画像に基づくカメラの位置姿勢の計測時刻の前後の２軸角度計の計測値を取得し、線形補間によりカメラ位置姿勢の計測時刻と一致する２軸角度計の計測値を取得しても良い。

又、２軸角度計の計測値を常に保持する必要はなく、カメラの位置姿勢の計測時刻の前後２地点の計測値が最低限あれば良い。又、補間を用いずに、カメラの計測時刻と２軸角度計の計測時刻との差分が十分小さいという仮定をして、２つの時刻が最も近い１点の計測値を用いても良い。

尚、実施形態１では、図１に示すように利用者がコントローラでＡＧＶを操作しながら地図を生成する方法について説明したが、既存の地図がある状態で自律移動する場合においても本発明を適用することができる。

例えば、実施形態１の方法で地図を作成し、同じ経路をＡＧＶで自律移動しているときに新たに地図要素を追加する必要がある場合に、センサの計測値に基づいて拘束条件を設定して、地図を補正しても良い。これにより、自律移動する際に追加する地図要素についても精度を向上させることが可能であり、当該地図に基づくカメラ位置姿勢の計測精度が向上する。

（実施形態２）
実施形態１では、本発明を、水平面上を移動するＡＧＶに適用する場合について述べたが、実施形態２では、本実施形態の方法をドローンに適用する事例について説明する。実施形態２においても実施形態１と同様に撮像装置１０であるカメラの画像を入力としてＳＬＡＭを実施する情報処理装置の例について説明する。

ドローンである移動体１の外部に配置されたセンサ１４により、ドローンの移動時の位置姿勢を計測する。そして、センサの座標系（以降、センサ座標系と記載する）で計測した撮像装置の位置姿勢情報を、ＳＬＡＭの計測で扱う撮像装置の座標系（以降、ＳＬＡＭ座標系と記載する）における撮像装置の位置姿勢情報に変換して地図補正時の拘束条件として利用する。

図６は、実施形態２に係る情報処理装置１００を含む移動体１の機能ブロック図である。実施形態２では撮像装置の拘束条件を判定するためのセンサ１４は、移動体の外部にあり、移動体に取り付けた光学マーカを画像認識することにより、移動体の位置姿勢を計測する。それ以外の機能ブロックの構成は図２の構成と同じである。

又、実施形態１では、ＡＧＶのように水平や平面などの移動体の移動の条件が制限されている場合に拘束条件を判定している。それに対して、実施形態２においては、ドローンのように位置及び姿勢の６次元のパラメータが任意の値をとりうる場合の拘束条件を判定する点が実施形態１と異なる。

実施形態２における処理全体の手順は、実施形態１で説明した図４、図５と同じであるが、各処理の中身が異なるため各々の処理について説明する。尚、ステップＳ１０１～ステップＳ１０３の処理は実施形態１と同じなので説明を省略する。

ステップＳ１０４では、センサ情報入力部１５が、ステップＳ１０２のカメラの撮影と同期するタイミングで、モーションキャプチャのシステムの計測値（ＳＬＡＭ座標系におけるカメラの六次元の位置姿勢のパラメータ）を取得する。尚、モーションキャプチャシステムは移動体１の外部に設けられており、移動体１に設けた光学マーカを認識することにより、移動体１の移動軌跡又は位置姿勢を外部から計測している。

このように、センサは、移動体の移動軌跡又は位置姿勢を移動体の外部から計測するものであっても良い。そして拘束条件判定は、センサが取得した計測値に基づいて拘束条件の重みを設定するものであっても良い。ステップＳ１０４で移動体１の位置姿勢を取得したらステップＳ１０５に進む。

ステップＳ１０５では、実施形態１と同様に、地図要素追加部１３が、地図要素の追加の必要の有無を判定する。尚、地図要素追加の判定方法は非特許文献１に記載の方法を用いる。地図要素の追加が必要であれば、ステップＳ１０６に進む。必要でなければ、ステップＳ１０８に進む。

ステップＳ１０６では、地図要素追加部１３が、新たな地図要素を作成し、地図保持部１１０が保持する地図情報に地図要素を追加する。非特許文献１に記載の方法で地図要素の追加を行い、ステップＳ１０４で入力したモーションキャプチャシステムによる移動体１の位置姿勢の計測値も併せて地図要素に登録する。地図追加が完了したらステップＳ１０７に進む。

ステップＳ１０７では、実施形態１と同様に、情報処理装置１００がステップＳ１０６で追加した地図要素に対して拘束条件を追加する。本ステップの処理の詳細については後述する。拘束条件の追加が完了すれば、ステップＳ１０８に進む。

ステップＳ１０８では、実施形態１と同様に、地図補正部１１４が、ステップＳ１０６が追加した最新の地図要素と、地図保持部が保持する過去に取得した地図要素のうち新しい順に所定数の地図要素と、を取得する。取得した地図要素を対象に、ステップＳ１０７で設定した拘束情報に基づいて、複数の地図要素に含まれる位置姿勢と特徴点群の三次元座標を補正する。

具体的には、地図要素が拘束情報を保持している場合、地図要素に含まれるカメラの位置姿勢が、地図要素に含まれるモーションキャプチャにより計測したカメラの位置姿勢に一致するように位置姿勢を補正する。

取得したすべての地図要素を対象に、ステップＳ１０７で設定したカメラの位置姿勢の六次元のパラメータの変化量を０にするという拘束条件を含めて、非特許文献１に記載のローカルバンドル調整を実施する。そして、地図要素に含まれる位置姿勢と特徴点群の三次元座標を補正する。補正が完了したらステップＳ１０９に進む

ステップＳ１０９では、実施形態１と同様に、システムを終了するか否かの判断を行う。本実施形態においては、移動体が目的地に到着した場合にコントローラ３００がシステムを終了するコマンドを入力したか否か判定する。終了コマンドが入力された場合にはシステムを終了し、そうでなければステップＳ１０２に戻り、処理を継続する。

ステップＳ２０１では、センサ計測値取得部１１５は、モーションキャプチャシステムからカメラの位置姿勢情報（位置を表す三次元のパラメータ、姿勢を表す三次元のパラメータ）を取得する。尚、ここで取得する位置姿勢情報はＳＬＡＭ座標系で表現されるものである。

ここで、モーションキャプチャシステムが計測したセンサ座標系の位置姿勢情報をＳＬＡＭ座標系での位置姿勢情報に変換するには、事前の校正によりセンサ座標系からＳＬＡＭ座標系への変換行列を求めておく。そして、センサが計測した位置姿勢情報に上記変換行列を適用することにより変換する。

ステップＳ２０２では、拘束条件判定部１１１が、ステップＳ２０１においてカメラの位置姿勢情報が取得できているか否かを判定し、Ｙｅｓの場合は１（真）を、Ｎｏの場合は０（偽）を出力する。尚、ここでいう位置姿勢が取得できない場合というのは、例えば、ドローンの光学マーカとモーションキャプチャシステムとの間に遮蔽物などが介入する場合などである。

ステップＳ２０３では、ステップＳ２０２で出力した判定結果が１（真）であれば、ステップＳ１０５に進み、０（偽）であれば、ステップＳ２０６に進む。

ステップＳ２０４では、拘束情報生成部１１２が、ステップＳ２０１で入力したカメラの六次元の位置姿勢情報、カメラの六次元の位置姿勢パラメータの地図補正時の変化量を０にするフラグを含む拘束情報データセットを生成し、ステップＳ２０５に進む。

ステップＳ２０５は、地図補正拘束条件設定部１１３が、地図保持部１１０に含まれる最新の地図要素を取得し、ステップＳ２０４で生成した拘束情報を表すデータセットを地図要素に追加し、処理全体を終了する。

以上のように、実施形態２では、移動体の任意の動きに合わせて撮像装置の位置姿勢の拘束条件を設定することで、ＳＬＡＭの蓄積誤差に起因する位置姿勢の誤差を抑制することができる。従って、高精度な三次元地図を生成することが可能となり、当該地図に基づく位置姿勢計測の精度が向上する。

尚、実施形態２では、拘束条件を課すか課さないかの二択で拘束条件を設定していたが、拘束の重みを適応的に変更しても良い。例えば、移動体１に取り付けた光学マーカを撮像するためにモーションキャプチャシステムに設けた俯瞰カメラと移動体に搭載された撮像装置の距離が大きくなるほど、拘束の重みを小さくしても良い。

或いは、上記俯瞰カメラで検出した光学マーカの検出位置の分散から算出した撮像装置の位置姿勢の分散が大きいほど、各パラメータの拘束の重みを小さくしても良い。又、俯瞰カメラの場合、遮蔽物によりマーカの一部が遮蔽される場合があるので、検出されたマーカの特徴点数に応じて各パラメータの拘束の重みを算出しても良い。

尚、実施形態２では、移動体１に取り付けた光学マーカを俯瞰カメラで認識することでセンサ座標系における撮像装置の位置姿勢を推定する例を説明したが、光学マーカを使わずに機械学習により移動体等の物体を認識して撮像装置の位置姿勢を推定しても良い。或いは、移動体の三次元モデルを保持しておいて、俯瞰カメラから得られる環境の三次元情報とマッチングすることで撮像装置の位置姿勢を推定しても良い。

尚、上述の実施形態においては自律移動体に制御システムを適用した例について説明した。しかし、本実施形態の移動体は、ＡＧＶ（Ａｕｔｏｍａｔｉｃ Ｇｕｉｄｅｄ Ｖｅｈｉｃｌｅ）やＡＭＲ（Ａｕｔｏｎｏｍｏｕｓ Ｍｏｂｉｌｅ Ｒｏｂｏｔ）などの自律移動体に限らない。例えば自動車、列車、船舶、飛行機、ロボット、ドローンなどの移動をする移動装置であればどのようなものであってもよい。また、本実施形態の制御システムは一部がそれらの移動体に搭載されていても良いし、搭載されていなくても良い。

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

（構成１）移動体に搭載された撮像装置が撮像したシーンの三次元位置情報と、前記撮像装置の位置及び姿勢と、前記撮像装置の位置及び姿勢の拘束条件を含む、複数の地図要素から構成される三次元地図を保持する地図保持手段と、前記拘束条件を判定するためのセンサの計測値を取得するセンサ計測値取得手段と、前記センサ計測値取得手段が取得した計測値に基づいて、前記拘束条件を判定する拘束条件判定手段と、前記拘束条件判定手段が判定した前記拘束条件に基づいて、前記三次元地図を補正する地図補正手段と、を備えることを特徴とする情報処理装置。

（構成２）前記センサは前記撮像装置の重力軸方向に対する傾斜角度を計測するものであり、前記拘束条件判定手段は、前記センサ計測値取得手段が取得した計測値に基づいて前記撮像装置が水平面上を移動している度合い又は水平方向を移動している度合いに基づいて前記拘束条件の重みを変更することを特徴とする構成１の情報処理装置。

（構成３）前記センサは周囲の物体との距離を計測するものであり、
前記拘束条件判定手段は、前記センサ計測値取得手段が取得した計測値に基づいて、前記撮像装置と周囲の物体との距離を算出し、算出された距離に基づいて前記拘束条件を判定することを特徴とする構成１又は２に記載の情報処理装置。

（構成４）前記撮像装置が撮影した画像を入力する入力手段と、前記入力手段が入力した前記画像から物体を認識する物体認識手段と、を更に有し、前記拘束条件判定手段は、前記センサ計測値取得手段が取得した計測値に基づいて、前記物体認識手段が認識した物体までの距離を算出し、算出された距離に基づいて前記拘束条件を設定することを特徴とする構成１～３のいずれか１つに記載の情報処理装置。

（構成５）前記センサは、前記移動体の制御情報を計測するものであり、
前記拘束条件判定手段は前記センサ計測値取得手段が取得した計測値に基づいて、前記拘束条件の重みを設定することを特徴とする構成１～４のいずれか１つに記載の情報処理装置。

（構成６）前記センサは、前記移動体の移動軌跡又は位置姿勢を前記移動体の外部から計測するものであり、前記拘束条件判定手段は、前記センサ計測値取得手段が取得した計測値に基づいて、前記拘束条件の重みを設定することを特徴とする構成１～５のいずれか１つに記載の情報処理装置。

（構成７）前記撮像装置が撮影した画像を入力する入力手段と、前記画像から検出した特徴点及び、前記三次元地図に基づいて前記撮像装置の位置姿勢を算出する位置姿勢算出手段と、前記位置姿勢算出手段が推定した位置姿勢と、前記画像から検出した前記特徴点を前記地図要素として前記三次元地図に追加する地図要素追加手段と、を更に有し、前記拘束条件判定手段は、前記地図要素追加手段が追加した前記地図要素の前記拘束条件を設定することを特徴とする構成１～６のいずれか１つに記載の情報処理装置。

（構成８）前記センサ計測値取得手段は複数の前記センサの計測値を取得し、前記拘束条件判定手段は、前記センサ計測値取得手段が取得した複数のセンサ計測値に基づいて複数の前記拘束条件を設定し、前記地図補正手段は前記拘束条件判定手段が設定した複数の前記拘束条件の組み合わせに基づき前記三次元地図を補正することを特徴とする構成１～７のいずれか１つに記載の情報処理装置。

（方法）移動体に搭載された撮像装置が撮像したシーンの三次元位置情報と該撮像装置の位置及び姿勢と、該撮像装置の位置及び姿勢の拘束条件を地図要素として含む、複数の前記地図要素から構成される三次元地図を保持する地図保持ステップと、前記拘束条件を判定するためのセンサの計測値を取得するセンサ計測値取得ステップと、前記センサ計測値取得ステップで取得した計測値に基づいて、前記拘束条件を判定する拘束条件判定ステップと、前記拘束条件判定ステップが判定した前記拘束条件に基づいて、前記三次元地図を補正する地図補正ステップと、を備える情報処理方法。

（プログラム）コンピュータを、構成１～８の何れか１つに記載の情報処理装置の各手段として機能させるためのプログラム。

尚、本発明は、前述した実施形態の機能を実現するソフトウェアのプログラムコード（制御プログラム）を記録した記憶媒体を、情報処理装置等に供給することによって実現してもよい。そして、そのシステムあるいは装置のコンピュータ（又はＣＰＵやＭＰＵ）が記憶媒体に格納されたコンピュータ読取可能なプログラムコードを読み出し実行することによっても達成される。
その場合、記憶媒体から読み出されたプログラムコード自体が前述した実施形態（実施例）の機能を実現することになり、そのプログラムコードを記憶した記憶媒体は本発明を構成することになる。

１：移動体
１０：撮像装置
１１：撮像情報入力部
１２：位置姿勢算出部
１３：地図要素追加部
１４：センサ
１５：センサ情報入力部
１６：移動指令取得部
１７：移動制御部
１００：情報処理装置
１１０：地図保持部
１１１：拘束条件判定部
１１２：拘束情報生成部
１１３：地図補正拘束条件設定部
１１４：地図補正部
１１５：センサ計測値取得部
２００利用者
３００コントローラ

Claims (10)

1. 移動体に搭載された撮像装置が撮像したシーンの三次元位置情報と、前記撮像装置の位置及び姿勢と、前記撮像装置の位置及び姿勢の拘束条件を含む、複数の地図要素から構成される三次元地図を保持する地図保持手段と、
   前記拘束条件を判定するためのセンサの計測値を取得するセンサ計測値取得手段と、
   前記センサ計測値取得手段が取得した計測値に基づいて、前記拘束条件を判定する拘束条件判定手段と、
   前記拘束条件判定手段が判定した前記拘束条件に基づいて、前記三次元地図を補正する地図補正手段と、を備えることを特徴とする情報処理装置。
2. 前記センサは前記撮像装置の重力軸方向に対する傾斜角度を計測するものであり、
   前記拘束条件判定手段は、前記センサ計測値取得手段が取得した計測値に基づいて前記撮像装置が水平面上を移動している度合い又は水平方向を移動している度合いに基づいて前記拘束条件の重みを変更することを特徴とする請求項１に記載の情報処理装置。
3. 前記センサは周囲の物体との距離を計測するものであり、
   前記拘束条件判定手段は、前記センサ計測値取得手段が取得した計測値に基づいて、前記撮像装置と周囲の物体との距離を算出し、算出された距離に基づいて前記拘束条件を判定することを特徴とする請求項１に記載の情報処理装置。
4. 前記撮像装置が撮影した画像を入力する入力手段と、
   前記入力手段が入力した前記画像から物体を認識する物体認識手段と、
   を更に有し、
   前記拘束条件判定手段は、前記センサ計測値取得手段が取得した計測値に基づいて、前記物体認識手段が認識した物体までの距離を算出し、算出された距離に基づいて前記拘束条件を設定することを特徴とする請求項３に記載の情報処理装置。
5. 前記センサは、前記移動体の制御情報を計測するものであり、
   前記拘束条件判定手段は前記センサ計測値取得手段が取得した計測値に基づいて、前記拘束条件の重みを設定することを特徴とする請求項１に記載の情報処理装置。
6. 前記センサは、前記移動体の移動軌跡又は位置姿勢を前記移動体の外部から計測するものであり、
   前記拘束条件判定手段は、前記センサ計測値取得手段が取得した計測値に基づいて、前記拘束条件の重みを設定することを特徴とする請求項１に記載の情報処理装置。
7. 前記撮像装置が撮影した画像を入力する入力手段と、
   前記画像から検出した特徴点及び、前記三次元地図に基づいて前記撮像装置の位置姿勢を算出する位置姿勢算出手段と、
   前記位置姿勢算出手段が推定した位置姿勢と、前記画像から検出した前記特徴点を前記地図要素として前記三次元地図に追加する地図要素追加手段と、
   を更に有し、
   前記拘束条件判定手段は、前記地図要素追加手段が追加した前記地図要素の前記拘束条件を設定することを特徴とする請求項１に記載の情報処理装置。
8. 前記センサ計測値取得手段は複数の前記センサの計測値を取得し、
   前記拘束条件判定手段は、前記センサ計測値取得手段が取得した複数のセンサ計測値に基づいて複数の前記拘束条件を設定し、
   前記地図補正手段は前記拘束条件判定手段が設定した複数の前記拘束条件の組み合わせに基づき前記三次元地図を補正することを特徴とする請求項１に記載の情報処理装置。
9. 移動体に搭載された撮像装置が撮像したシーンの三次元位置情報と該撮像装置の位置及び姿勢と、該撮像装置の位置及び姿勢の拘束条件を地図要素として含む、複数の前記地図要素から構成される三次元地図を保持する地図保持ステップと、
   前記拘束条件を判定するためのセンサの計測値を取得するセンサ計測値取得ステップと、
   前記センサ計測値取得ステップで取得した計測値に基づいて、前記拘束条件を判定する拘束条件判定ステップと、
   前記拘束条件判定ステップが判定した前記拘束条件に基づいて、前記三次元地図を補正する地図補正ステップと、を備える情報処理方法。
10. コンピュータを、請求項１～８のいずれか１項に記載の情報処理装置の各手段として機能させるためのプログラム。
    
    
    

Images