JP7760321B2 - 情報処理装置、情報処理装置の制御方法及びプログラム - Google Patents

Description

本発明は、情報処理装置、情報処理装置の制御方法及びプログラムに関し、特に、移動体及び移動体の周辺機器の制御に関するものである。

無人搬送車、例えば、ＡＧＶ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｖｅｈｉｃｌｅ）などの移動体を、工場や物流倉庫といった環境内において自動で移動させる技術が提案されている。自動で移動させる場合において、実空間の地図作成と、実空間内での自己位置姿勢計測を行うことがある。そのための手法としては、例えば、ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）法が知られている。位置姿勢計測のためのセンサデータとして、カメラで撮影された画像データ、ＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ）で計測されたレーザ画像データがある。あるいは、ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）で計測された慣性計測データ（加速度、角速度など）が用いられる。

特許文献１では、２種類のセンサデータを用いて位置姿勢計測を行い、第１のセンサデータを用いた位置姿勢計測の信頼度が下がった場合に、第２のセンサデータの位置姿勢計測の結果を選択することで、移動体の走行を安定化する手法が提案されている。

国際公開第２０１９／０２６７６１号

しかしながら、特許文献１の方法では、選択した第２のセンサデータを用いた位置姿勢計測の信頼度が時間や距離に従って低下する場合、移動体を安定して運行できないという課題があった。

本発明は、上記問題に鑑みてなされたものであり、移動体を安定して運行させることができる情報処理装置を提供することを目的とする。

上記の目的を達成するために、本発明における情報処理装置は、第１のセンサデータを用いた第１の位置姿勢計測と第２のセンサデータを用いた第２の位置姿勢計測とに基づく位置姿勢計測の結果に基づいて移動体及び前記移動体の周辺機器の制御内容を決定する情報処理装置であって、前記位置姿勢計測における前記第２の位置姿勢計測に依存する度合いを示す依存度を取得する依存度取得手段と、前記依存度取得手段が取得した依存度に関する蓄積値の大きさに基づいて、前記位置姿勢計測の結果の信頼度を算出する信頼度算出手段と、前記信頼度算出手段が算出した前記信頼度に基づいて、前記制御内容を決定する制御内容決定手段と、を備え、前記位置姿勢計測は、前記第１の位置姿勢計測の精度が所定の閾値以上の場合は前記第２の位置姿勢計測を使用せず、前記第１の位置姿勢計測の精度が所定の閾値未満の場合は前記第２の位置姿勢計測を合わせて使用し、
前記第２の位置姿勢計測は、前記第１の位置姿勢計測に比べて、時間または距離の経過に応じて蓄積する誤差がより大きく、前記依存度に関する蓄積値は、前記第２の位置姿勢計測を使い始めてから蓄積した値である、ことを特徴とする。

本発明によれば、移動体を安定して運行させることができる。

第１実施形態における情報処理装置を備える移動体システムの構成例を示す図。 第１実施形態における情報処理装置のハードウェア構成を示す図。 第１実施形態における情報処理装置の処理手順を示すフローチャート。 第２実施形態における情報処理装置を備える移動体システムの構成例を示す図。 第２実施形態における情報処理装置の処理手順を示すフローチャート。 第３実施形態における情報処理装置を備える移動体システムの構成例を示す図。 第３実施形態における情報処理装置の処理手順を示すフローチャート。 第４実施形態における表示画面を示す図。

以下、本発明の実施形態について、図面を参照して説明する。なお、以下の実施形態は特許請求の範囲に関る本発明を限定するものではなく、また、本実施形態で説明されている特徴の組み合わせの全てが本発明の解決手段に必須のものとは限らない。なお、以下の実施形態において、信頼度や精度等が「向上する」とは、信頼性や精度等が「高まる」ことをいう。

［第１実施形態］
本実施形態では、本発明の方法を、荷物を搬送する移動体における通知制御に適用した例を説明する。本実施形態における移動体は、所定の経路を繰り返し自動走行する。移動体の自動走行時、移動体に搭載したカメラで撮影した画像データから検出した画像特徴点と、ＩＭＵで計測した加速度及び角速度を用いてＳＬＡＭ技術により位置姿勢計測を行う。以下、ＩＭＵの出力データをまとめて「慣性計測データ」と呼ぶ。

一般にカメラ画像に基づくＳＬＡＭにおいては、照明の消灯・点灯のような照度変化や、カメラ周囲の被写体の配置変化などにより、位置姿勢計測の信頼度が低下する可能性がある。その場合、位置姿勢計測における慣性計測データへの依存度が高まる。しかし、慣性計測データを用いた位置姿勢計測は、例えば、加速度に対して単位時間による２回積分を行い、変位量を算出して位置を推定するため、時間が経過する程、誤差が増大する。

そこで本実施形態では、移動体の位置姿勢計測において慣性計測データに依存する度合いである依存度を取得する。また、依存度に関する蓄積値を取得する。さらに、依存度に基づいて位置姿勢計測への誤差蓄積の少なさを表す信頼度を算出する。そして、位置姿勢計測の信頼度が低下した場合に、警告通知を行う。

本実施形態においては、移動体が平面上を移動する。したがって、移動体の位置姿勢とは、２次元平面上に任意の位置を表す２パラメータ（Ｘ、Ｙ）、および移動体の姿勢を表す１パラメータ（θ）を合わせた３パラメータのことである。

図１は、第１実施形態における情報処理装置を備える移動体システムの構成例を示す図である。移動体システム１００は、第１のセンサデータ取得手段１０１、第２のセンサデータ取得手段１０２、位置姿勢計測手段１０３、通知手段１０４、移動手段１０５、表示手段１０６、情報処理装置１１０から構成される。本実施形態における移動体システム１００は、情報処理装置１１０に制御される移動体の一例であり、通知手段１０４、移動手段１０５、表示手段１０６は、移動体の周辺機器の一例である。

情報処理装置１１０は、依存度取得手段１１１、信頼度算出手段１１２、制御内容決定手段１１３から構成される。

第１のセンサデータ取得手段１０１は、第１のセンサから第１のセンサデータを取得する。本実施形態において、第１のセンサはカメラとし、第１のセンサデータは各画素に輝度値が記録された画像データとする。

第２のセンサデータ取得手段１０２は、第２のセンサから第２のセンサデータを取得する。本実施形態において、第２のセンサはＩＭＵとし、第２のセンサデータは加速度および角速度とする。

位置姿勢計測手段１０３は、第１のセンサデータと第２のセンサデータに基づき、移動体システム１００の位置姿勢を計測する。位置姿勢計測の詳細については後述する。

通知手段１０４は、制御内容決定手段１１３から、制御内容決定手段１１３が決定した通知内容を取得し、ユーザに通知する。ここでの通知内容は、主に、信頼度の大きさを示す通知内容であるが、これに限るものではない。

移動手段１０５は、制御内容決定手段１１３から、制御内容決定手段１１３が決定した移動内容を取得し、移動体システム１００の走行を制御する。

表示手段１０６は、制御内容決定手段１１３から、制御内容決定手段１１３が決定した表示内容を取得し、画面に表示する。

依存度取得手段１１１は、位置姿勢計測手段１０３による位置姿勢計測における第２のセンサデータへの依存度を取得する。

信頼度算出手段１１２は、依存度取得手段１１１が取得した依存度に基づいて位置姿勢計測に関する信頼度を算出する。

制御内容決定手段１１３は、本実施形態において、移動体システム１００に関する通知内容等の制御内容を決定する。具体的には、通知手段１０４に対する通知内容、移動手段１０５に対する移動内容、表示手段１０６に対する表示内容を決定する。また、本実施形態における移動手段１０５に対する移動内容として、制御内容決定手段１１３は、移動体の進行、移動体の速度、移動体の姿勢、または移動体の経路、のうち、少なくともいずれかを決定する。また、表示手段１０６に対する表示内容として、制御内容決定手段１１３は、信頼度の大きさを識別可能な表示内容を決定する。なお、制御内容決定手段１１３の制御内容は、これらに限るものではない。

図２は、第１実施形態における情報処理装置１１０のハードウェア構成を示す図である。ＣＰＵ１１は、システムバス２０に接続された各種デバイスの制御を行う。ＲＯＭ１２は、ＢＩＯＳのプログラムやブートプログラムを記憶する。ＲＡＭ１３は、ＣＰＵ１１の主記憶装置として使用される。外部メモリ１４は、情報処理装置１１０が処理するプログラムを格納する。入力部１５は、キーボードやマウス、ロボットコントローラーであり、情報等の入力に係る処理を行う。表示部１６は、ＣＰＵ１１からの指示に従って情報処理装置１１０の演算結果を表示装置に出力する。なお、表示装置は液晶表示装置やプロジェクタ、ＬＥＤインジケーターなど、種類は問わない。また、システムバス２０には、Ｉ／Ｏ１７が接続される。

図３は、第１実施形態における情報処理装置１１０の処理手順を示すフローチャートである。

ステップＳ３０１では、情報処理装置１１０の初期化を行う。具体的には、信頼度算出手段１１２において信頼度を算出する際に使用する依存度に関する閾値を読み込む。また、制御内容決定手段１１３において制御内容を決定する際に使用する信頼度と制御内容の組が複数列挙されたリストなどの設定を読み込む。

ステップＳ３０２では、依存度取得手段１１１が、位置姿勢計測手段１０３からある一時点での位置姿勢計測における第２のセンサデータへの依存度を取得する。依存度とは、位置姿勢計測手段１０３が実施する位置姿勢計測において、慣性計測データを用いた位置姿勢計測に依存する度合いを示す値である。取得する依存度の詳細については後述する。

ステップＳ３０３では、信頼度算出手段１１２が、ステップＳ３０２において取得した依存度に基づいて信頼度を算出する。信頼度とは、位置姿勢計測手段１０３による計測結果における誤差蓄積の少なさを示す値である。信頼度の算出方法の詳細については後述する。

ステップＳ３０４では、制御内容決定手段１１３が、ステップＳ３０３において算出した信頼度に基づいて、前述した信頼度と制御内容の組から構成されるリストを参照し、移動体システム１００に関する制御内容を決定する。本実施形態において、制御内容決定手段１１３は、前記リストを参照し、算出した信頼度が所定の値より低くなった場合、位置姿勢計測の信頼度が低下している旨を示す警告メッセージを通知することを決定する。

ステップＳ３０５では、情報処理装置１１０の処理全体を終了するか否かを判定する。具体的には、移動体システム１００が目的地に到達して自動走行を終了する場合に終了する。そうでなければ、ステップＳ３０２に戻り、処理を継続する。

本実施形態において、位置姿勢計測手段１０３は、画像データと慣性計測データを用いて位置姿勢計測を行う。ここで、画像データとは、第１のセンサであるカメラによって撮影された第１のセンサデータである。また、慣性計測データとは、第２のセンサであるＩＭＵによって計測された第２のセンサデータである。

画像データと慣性計測データを用い、これらに関する位置姿勢計測の方法として、本実施形態では、ＯＲＢ－ＳＬＡＭ３方式を利用する。具体的には、Ｃｏｒｎｅｌｌ Ｕｎｉｖｅｒｓｉｔｙにおいて、Ｃａｒｌｏｓ Ｃａｍｐｏｓ ｅｔ．ａｌ，により示された、次の方法による。すなわち、ＯＲＢ－ＳＬＡＭ３：Ａｎ Ａｃｃｕｒａｔｅ Ｏｐｅｎ－Ｓｏｕｒｃｅ Ｌｉｂｒａｒｙ ｆｏｒ Ｖｉｓｕａｌ， Ｖｉｓｕａｌ－Ｉｎｅｒｔｉａｌ ａｎｄ Ｍｕｌｔｉ－Ｍａｐ ＳＬＡＭ．において示された方法による。

本方法では、位置姿勢計測用地図として保存された複数の画像特徴の中から、カメラで撮影された画像データから検出した画像特徴とマッチングを行う。次に、画像特徴位置と慣性計測データを組み合わせて算出した位置姿勢の差の総和（残差）が最小となる位置姿勢を計測する。このとき、画像データから検出した画像特徴点の数を慣性計測データに対する依存度として算出する。

位置姿勢計測用地図は、撮影された画像データ、撮影時のカメラの位置姿勢、前記画像データから検出した画像特徴点、前記画像特徴点の３次元位置の集合とする。画像特徴情報とは、画像特徴の特徴点、画像上での二次元座標のことである。画像特徴とは、画像中の角など幾何学的な構造を指し示す特徴点として検出する。画像データ、撮影時の位置姿勢、撮影画像から検出した画像特徴点、該特徴の３次元位置の１セットを、キーフレームと呼ぶこととする。

なお、本実施形態において、位置姿勢計測用地図は、移動体システム１００が自動走行を開始する前に位置姿勢計測手段１０３によって作成される。また、位置姿勢計測用地図は、位置姿勢計測手段１０３が保持するものとする。

また、本実施形態において、第１のセンサであるカメラは移動体システム１００に固定されている。移動体システム１００の移動・回転と連動してカメラが移動・回転するため、移動体システム１００の位置姿勢はカメラの位置姿勢と連動している。撮影される画像データはカメラの位置姿勢に依存するため、移動体システム１００の位置または姿勢の変更は、画像データを用いた位置姿勢計測の結果に影響する。

また、本実施形態において、移動体システム１００の自動走行中、第１のセンサデータ取得手段１０１は、所定の時間間隔で画像データを繰り返し取得する。第２のセンサデータ取得手段１０２は、所定の時間間隔で慣性計測データを繰り返し取得する。位置姿勢計測手段１０３は、所定の間隔（例えば、位置姿勢計測用地図として保持されたキーフレーム毎）で、その時点で取得した最新の画像データと慣性計測データを用いて、位置姿勢計測を繰り返し実施する。

本実施形態において、依存度取得手段１１１が位置姿勢計測手段１０３から取得する第２のセンサデータへの依存度の高さは、画像特徴点の数の少なさを基準として算出される。具体的には、移動体システム１００の自動走行中、位置姿勢計測手段１０３によるある一時点での位置姿勢計測において、その時点で取得した画像データから検出した画像特徴点の数とする。そして、画像特徴点の数が多い程、依存度が低く、画像特徴点の数が少ない程、依存度が高くなるように算出する。この理由は、画像データに関する位置姿勢計測の精度は、画像データの計測時の移動体の位置姿勢に依存することによる。具体的には、画像データを用いた位置姿勢計測の精度は、画像特徴点の数に依存し、画像特徴点の数が少ない程、画像データを用いた位置姿勢計測の精度が低下し、結果として慣性計測データを用いた位置姿勢計測に依存するからである。

本実施形態において、信頼度算出手段１１２が算出する信頼度は、依存度取得手段１１１が取得した依存度が所定の閾値を超えた状態が継続する時間が大きくなる程、信頼度が低くなるように算出する。具体的には、最も低い信頼度の値を０．０、最も高い信頼度の値を１．０とし、信頼度の初期値を１．０とする。依存度が所定の閾値を超えた時間を起点に、その状態が継続する間、所定の時間間隔が経過する毎に信頼度を０．１ずつ減らすものとする。もし、依存度が前記所定の時間間隔で２回連続して前記所定の閾値を下回った場合、信頼度を初期値１．０に戻す。

第１実施形態の方法により、位置姿勢計測の信頼度が低下した場合、移動体システム１００を安定して運行できない状況になる前に、情報処理装置がユーザに警告通知を出すように制御する。これにより、結果として移動体を安定して運行することができる。

本実施形態において、第２のセンサデータへの依存度は、画像データから検出した画像特徴点の数を用いて算出したが、これに限らず、画像特徴点の空間的な偏りを用いても良い。依存度の算出方法としては、画像特徴点が空間的に広く分布している場合、画像データを用いた位置姿勢計測の精度が高くなるため、依存度が低くなるように算出する。一方、画像特徴点が空間的に偏って分布している場合、画像データを用いた位置姿勢計測の精度が低くなるため、依存度が高くなるように算出する。

また、位置姿勢計測手段１０３から位置姿勢計測時に使用した設定値、または途中の計算結果を取得できれば、これに限らない。例えば、位置姿勢計測のバンドル調整時に算出する画像データに関する残差を用いても良い。依存度の算出方法としては、画像データに関する残差が小さい場合、画像データを用いた位置姿勢計測の精度が高くなるため、依存度が低くなるように算出する。一方、画像データに関する残差が大きい場合、画像データを用いた位置姿勢計測の精度が低くなるため、依存度が高くなるように算出する。

また、本実施形態で利用するＯＲＢ－ＳＬＡＭ３方式による位置姿勢計測において、画像データが占める割合と慣性計測データが占める割合を調整するための設定値を用いて、依存度を算出しても良い。依存度の算出方法としては、慣性計測データが占める割合を示す設定値が大きい程依存度が高く、設定値が小さい程依存度が低くなるように算出する。

また、位置姿勢計測時に算出する慣性計測データに関するバイアス補正の結果を用いても良い。ＩＭＵの特性上、ＩＭＵで計測した慣性計測データにはバイアス分が含まれており、本実施形態で利用するＯＲＢ－ＳＬＡＭ３方式の位置姿勢計測では、このバイアスを補正する処理を行う。しかし、画像データを用いた位置姿勢計測の精度が低下すると補正値の差異が大きくなるため、補正値の差異が大きい程、第２のセンサデータである慣性計測データへの依存度が高くなるように算出する。

本実施形態において、本実施形態において、信頼度算出手段１１２は、依存度に関する蓄積値が大きくなる程、信頼度がより低くなるように算出する。例えば、信頼度算出手段１１２は、依存度が所定の閾値を超えた状態が継続する時間が大きくなる程、信頼度が低くなるように算出する。しかしながら、これに限るものではない。すなわち、依存度の蓄積値である誤差蓄積が算出できれば他の方法でも良い。例えば、信頼度算出手段１１２は、依存度の和が大きくなる程、信頼度が低くなるように算出しても良い。具体的には、依存度が所定の閾値を超えた時点を起点に依存度の和を算出し、和の大きさに応じて、信頼度を一定割合減らすものとする。

また、信頼度算出手段１１２は、依存度が所定の閾値を超えた状態で移動する距離が大きくなる程、信頼度がより低くなるように算出しても良い。具体的には、位置姿勢計測手段１０３は、キーフレームを所定の距離毎に保持し、信頼度の初期値を１．０とする。そして、依存度が所定の閾値を超えた位置を起点に、その状態が継続する間、位置姿勢計測手段１０３が参照するキーフレーム毎に信頼度を０．１減らすものとする。もし、依存度が連続するキーフレームで前記所定の閾値を下回った場合、信頼度を１．０に戻す。

また、上述したように、信頼度算出手段１１２は、依存度の蓄積値や時間経過、および移動距離に応じて、信頼度を一定割合減らすとしたが、これに限らず、例えば信頼度を指数関数的に減らしても良い。また、信頼度を依存度の蓄積値の逆数となるように算出しても良い。

また、信頼度算出手段１１２は、依存度が所定の閾値を超えた時点を起点に、信頼度を一定割合減らすとしたが、所定の閾値は設けず、依存度の値の大きさに応じて適宜信頼度を減らすように算出しても良い。

本実施形態において、制御内容決定手段１１３は、信頼度が所定の値より低くなった場合、位置姿勢計測の信頼度が低下している旨を示す警告メッセージを通知することを決定したが、これに限るものではない。ユーザに信頼度の低下を通知する手段を備えれば他の方法でも良く、例えば警告音を鳴らす、または外部機器へ出力することを決定しても良い。

また、制御内容決定手段１１３は、決定した制御内容を保持することができれば、次回以降、信頼度が所定の値より高くなった際に、信頼度が回復した旨を通知する内容を決定しても良い。例えば、信頼度が所定の値より高くなった場合、位置姿勢計測の信頼度が向上している旨を示すメッセージを通知することを決定しても良い。同様に、制御内容決定手段１１３は、信頼度が所定の値より高くなった場合、警告音を停止することを決定しても良い。

本実施形態において、第１のセンサはカメラとし、第１のセンサデータは画像データとしたが、これに限るものではない。例えば第１のセンサはＬｉＤＡＲ、第１のセンサデータはＬｉＤＡＲによって計測される２次元または３次元の点群データであっても良い。

同様に、本実施形態において、第２のセンサはＩＭＵとし、第２のセンサデータは慣性計測データとしたが、これに限るものではない。すなわち、移動体システム１００が車輪を備えている場合、例えば第２のセンサは車輪の回転を計測するロータリーエンコーダ、第２のセンサデータはロータリーエンコーダによって計測される前記車輪の回転角度であっても良い。また、本実施形態において、第２のセンサデータとして加速度及び角速度をまとめた慣性計測データを用いたが、これに限るものではない。第２のセンサデータとして加速度だけ、または角速度だけを用いても良い。

２種類のセンサデータを用いた位置姿勢計測の方法として、疎結合（ルースカップリング）方式と、密結合（タイトカップリング）方式がある。疎結合方式は、それぞれの位置姿勢計測の結果を統合する方式であり、密結合方式は、２種類のセンサデータを統合して位置姿勢計測を実施する方式である。本実施形態において、位置姿勢計測手段１０３は密結合方式による位置姿勢計測を用いたが、これに限らず、疎結合方式を用いても良い。例えば、位置姿勢計測手段１０３は、画像データを用いた位置姿勢計測と、慣性計測データを用いた位置姿勢計測をそれぞれ行い、画像データを用いた位置姿勢計測の結果を、慣性計測データを用いた位置姿勢計測の結果で補正する。

［変形例１－１］
第１実施形態の変形例について説明する。本変形例では、本発明の方法を移動体における移動制御に適用した例を説明する。具体的には、位置姿勢計測の信頼度が低下した際に、移動体は減速、または停止する。

本変形例において、制御内容決定手段１１３が制御内容を決定する際に、信頼度と制御内容の組が複数列挙されたリストを使用する。リストには、信頼度の第１の値に対して走行を減速する制御内容、信頼度の第２の値に対して走行を停止する制御内容が記載される。

本変形例において、第１実施形態で説明した情報処理装置１１０の処理手順を示すフローチャートである図３との変更点について説明する。ステップＳ３０４では、制御内容決定手段１１３は、前記リストを参照する。そして、信頼度算出手段１１２が算出した信頼度が第１の値より低くなった場合、移動体システム１００の走行を減速することを決定する。一方、制御内容決定手段１１３は、信頼度が第２の値より低くなった場合、移動体システム１００の走行を停止することを決定する。また、制御内容決定手段１１３は、第２の値より低くなっていた信頼度が第２の値より高くなった場合、停止していた移動体システム１００の走行を再び開始することを決定する。同様に、制御内容決定手段１１３は、第１の値より低くなっていた信頼度が第１の値より高くなった場合、減速していた移動体システム１００の速度を回復することを決定する。

移動手段１０５は、制御内容決定手段１１３が決定した制御内容に従って、移動体システム１００の走行を制御する。

本変形例において、制御内容決定手段１１３は、信頼度に基づいて移動体システム１００の走行を減速、または停止することを決定したが、これに限るものではない。移動体システム１００が十分な広さの走行経路を確保できれば、例えば移動体システム１００の姿勢を変更しても良い。具体的には、制御内容決定手段１１３は、信頼度が所定の値より低くなった場合、移動体システム１００の姿勢を所定の角度で回転することを決定する。それに従って、移動手段１０５は、移動体システム１００の姿勢を変更する。以上の処理を、信頼度が所定の値より高くなるまで繰り返す。

［第２実施形態］
本実施形態では、本発明の方法を、荷物を搬送する移動体における移動制御に適用した例を説明する。変形例１－１では、位置姿勢計測の信頼度が低下した際に、移動体を減速、または停止することを決定することで、移動体が安定して運行できることを示した。

本実施形態では、移動体の位置姿勢と信頼度を組とする信頼度分布を保持し、位置姿勢計測の信頼度が低下した際に、移動体の現在位置近傍の信頼度を探索し、信頼度がより高くなる方向に向けて移動体の姿勢を変更する。これにより、位置姿勢計測の信頼度を向上させる方法について述べる。

図４は、第２実施形態における情報処理装置１１０を備える移動体システムの構成例を示す図である。第１実施形態で説明した情報処理装置を備える移動体システムの構成例を示した図１と同じモジュールについては説明を省略し、第１実施形態と異なるモジュールのみを説明する。

本実施形態において、情報処理装置１１０は、依存度取得手段１１１、信頼度算出手段１１２、制御内容決定手段１１３、位置姿勢取得手段４１１、信頼度分布保持手段４１２から構成される。

位置姿勢取得手段４１１は、位置姿勢計測手段１０３から位置姿勢計測の結果を取得する。

信頼度分布保持手段４１２は、位置姿勢計測手段１０３が取得した位置姿勢と、前記位置姿勢に対応する依存度に基づいて信頼度算出手段１１２が算出した信頼度の組から構成されるリストを保持する。以降、移動体の位置姿勢と信頼度とを関連させた組から構成される前記リストを信頼度分布リストと呼ぶ。

制御内容決定手段１１３は、信頼度分布保持手段４１２が保持した信頼度分布リストに基づいて、移動体システム１００に関する制御内容を決定する。本実施形態において、制御内容決定手段１１３は、移動体システム１００に関する移動内容を決定する。

図５は、第２実施形態における情報処理装置１１０の処理手順を示すフローチャートである。第１実施形態で説明した情報処理装置の処理手順を示すフローチャートである図３と同じステップについては説明を省略し、第１実施形態と異なるステップのみを説明する。なお、図５も図３と同様に、繰り返し処理を実施する。

ステップＳ３０１では、信頼度分布リストを初期化する。初期化後、信頼度分布リストは要素が０個の状態となる。

ステップＳ５０１では、位置姿勢取得手段４１１が、位置姿勢計測手段１０３から位置姿勢を取得する。

ステップＳ５０２では、信頼度分布保持手段４１２が、ステップＳ５０１において取得した位置姿勢と、ステップＳ３０３において前記位置姿勢に対応する依存度に基づいて算出した信頼度を組として信頼度分布リストに追加する。

ステップ５０３では、制御内容決定手段１１３が、ステップＳ５０２において新規追加した位置姿勢と信頼度の組と、これまでに保持した信頼度分布リストに基づいて、移動体システム１００の移動内容を決定する。移動内容の決定方法の詳細については後述する。

ステップ５０３において、制御内容決定手段１１３は、新規追加した信頼度が所定の値より低くなった場合、画像データを用いた位置姿勢計測の精度が向上する方向に移動体システム１００の姿勢を変更することを決定する。ここで、画像データを用いた位置姿勢計測の精度が向上する方向とは、信頼度分布リストにおける、所定の値より高い信頼度と組になっている位置姿勢に基づいて決定した方向をいう。具体的には、まず制御内容決定手段１１３は、信頼度分布リストから信頼度分布保持手段４１２が新規追加した位置姿勢と信頼度の組を取得する。もし、新規追加した信頼度が所定の値より低い場合、新規追加した位置姿勢における位置の近傍領域で、信頼度が所定の値より高い位置姿勢と信頼度の組が存在するか、信頼度分布リストを探索する。新規追加した位置の近傍領域は、新規追加した位置と、信頼度分布リスト中で参照した他の位置との差が所定の距離以内か否かで判断する。

もし、新規追加した位置の近傍領域で信頼度が所定の値より高い位置姿勢と信頼度の組が存在した場合、新規追加した位置において所定の角度ずつ回してカメラの視錐台を算出する。そして、信頼度が所定の値より高い位置姿勢におけるカメラの視錐台と最も重複が大きくなるように、移動体システム１００の姿勢を変更する。

もし、新規追加した位置の近傍領域で信頼度が所定の値より高い位置姿勢と信頼度の組が複数存在した場合、新規追加した位置姿勢との差異が最も少ない組を選択する。

もし、新規追加した位置の近傍領域で信頼度が所定の値より高い位置姿勢と信頼度の組が存在しない場合、制御内容決定手段１１３は、変形例１－１と同様に、移動体システム１００の走行を減速することを決定する。

本実施形態の方法により、位置姿勢計測の信頼度が低下した場合でも、移動体の姿勢変更によって位置姿勢計測の信頼度を向上して、移動体を安定して運行することができる。

本実施形態において、制御内容決定手段１１３は、信頼度が所定の値より高い位置姿勢におけるカメラの視錐台と最も重複が大きくなるように、移動体システム１００の姿勢を変更したが、これに限るものではない。当該位置姿勢における画像データから検出される画像特徴が撮影可能な姿勢を算出できれば、他の方法でも良い。例えば、当該位置姿勢における画像データの画像特徴点群が、新規追加した位置における視推台に含まれるように姿勢を変えても良い。または、当該位置姿勢における視推台を所定の奥行きで切断した平面が、新規追加した位置における視推台に含まれるように姿勢を変えても良い。

本実施形態において、制御内容決定手段１１３は、信頼度が所定の値より低くなった場合、画像データを用いた位置姿勢計測の精度が向上する方向に移動体システム１００の姿勢を変更することを決定したが、これに限らない。

例えば、制御内容決定手段１１３は、新規追加した信頼度が所定の値より低くなった場合、画像データを用いた位置姿勢計測の精度が向上する位置に移動体システム１００の位置を変更することを決定しても良い。ここで、画像データを用いた位置姿勢計測の精度が向上する位置とは、信頼度分布リストにおける、所定の値より高い信頼度と組になっている位置姿勢に基づいて決定した位置をいう。姿勢の変更と同様に、信頼度分布リストを探索し、新規追加した位置の近傍領域で信頼度が所定の値より高い位置姿勢と信頼度の組が存在した場合、該当の位置に移動するように移動体システム１００の位置を変更する。さらに、位置だけではなく、探索結果の姿勢とほぼ一致するように移動体システム１００の姿勢を変更しても良い。

また、制御内容決定手段１１３は、決定した制御内容を保持することができれば、次回以降、信頼度が所定の値より高くなった場合、移動体システム１００の姿勢を、変更前の姿勢に戻すことを決定しても良い。

本実施形態のステップＳ３０１において、信頼度分布リストは要素が０個の初期状態としたが、これに限らない。例えば、信頼度分布保持手段４１２が図５に示した処理の終了後も信頼度分布リストを保持し、移動体システム１００が次回も同一の２次元平面上を走行すれば、他の方法でも良い。例えば、前回の処理において保持された信頼度分布リストを初期化時に読込み、再利用しても良い。

［第３実施形態］
本実施形態では、本発明の方法を、荷物を搬送する移動体における移動制御に適用した例を説明する。第２実施形態では、位置姿勢と信頼度を組とする信頼度分布を保持し、位置姿勢計測の信頼度が低下した際に、近傍の信頼度分布を探索し、信頼度がより高くなる方向に向けて移動体の姿勢を変更することで、位置姿勢計測の信頼度を向上する方法を示した。

本実施形態では、位置姿勢と画像特徴を組とする特徴分布を保持し、位置姿勢計測の信頼度が低下した際に、近傍の画像特徴分布を探索し、信頼度がより高くなる経路に移動体の走行経路を変更する。これにより、位置姿勢計測の信頼度を向上する方法について述べる。

図６は、第３実施形態における情報処理装置１１０を備える移動体システムの構成例を示す図である。第２実施形態で説明した情報処理装置を備える移動体システムの構成例を示した図４と同じモジュールについては説明を省略し、第２実施形態と異なるモジュールのみを説明する。

情報処理装置１１０は、依存度取得手段１１１、信頼度算出手段１１２、制御内容決定手段１１３、位置姿勢取得手段４１１、信頼度分布保持手段４１２を有する。また、本実施形態においては、情報処理装置１１０は、特徴分布取得手段６１１を有する。

特徴分布保持手段６０１は、位置姿勢計測手段１０３から第１のセンサデータである画像データを用いた位置姿勢計測時に検出した画像特徴点の３次元位置の集合と、前記画像データの撮影時の位置姿勢を取得する。そして、特徴分布保持手段６０１は、位置姿勢と画像特徴点の３次元位置の集合の組から構成されるリストとして保持する。以降、前記リストを特徴分布リストと呼ぶ。

特徴分布取得手段６１１は、特徴分布保持手段６０１から特徴分布リストを取得する。

制御内容決定手段１１３は、信頼度分布保持手段４１２が保持した位置姿勢と信頼度の組と、特徴分布取得手段が取得した特徴分布リストに基づいて、移動体システム１００に関する移動内容を決定する。

図７は、第３実施形態における情報処理装置１１０の処理手順を示すフローチャートである。第２実施形態で説明した情報処理装置の処理手順を示すフローチャートである図５と同じステップについては説明を省略し、第２実施形態と異なるステップのみを説明する。なお、図７も図５と同様に、繰り返し処理を実施する。

ステップＳ７０１では、特徴分布取得手段６１１が、特徴分布保持手段６０１から特徴分布リストを取得する。

ステップＳ７０２では、制御内容決定手段１１３が、ステップＳ５０２において保持した位置姿勢と信頼度の組と、ステップＳ７０１において取得した特徴分布リストに基づいて、移動体システム１００の移動内容を決定する。移動内容の決定方法の詳細については後述する。

ステップＳ７０２において、制御内容決定手段１１３は、信頼度が所定の値より低くなった場合、画像データを用いた位置姿勢計測の精度が向上する経路に移動体システム１００の経路（走行経路）を変更することを決定する。

具体的には、まず制御内容決定手段１１３は、信頼度分布保持手段４１２が保持した位置姿勢と信頼度の組を取得する。もし、保持した信頼度が所定の値より低い場合、保持した位置姿勢における位置の近傍領域で、所定の数より多い画像特徴点が存在するか、特徴分布取得手段６１１が取得した特徴分布リストを探索する。保持した位置の近傍領域は、保持した位置と、特徴分布リスト中で参照した他の位置との差が所定の距離以内か否かで判断する。

もし、保持した位置の近傍領域で所定の数より多い画像特徴点の組が存在した場合、該当の位置を走行するように、移動体システム１００の走行経路を変更する。現在の位置の近傍領域で所定の数より画像特徴点の組が複数存在した場合は、現在の位置姿勢との差異が最も少ない画像特徴点の組を選択する。
保持した位置の近傍領域で信頼度が所定の値より多い画像特徴点の組が存在しない場合、制御内容決定手段１１３は、変形例１－１と同様に、移動体システム１００の走行を減速することを決定する。

本実施形態の方法により、位置姿勢計測の信頼度が低下した場合でも、移動体の経路変更によって位置姿勢計測の信頼度を向上して、移動体を安定して運行することができる。

本実施形態において、制御内容決定手段１１３は、決定した制御内容を保持することができれば、次回以降、信頼度が所定の値より高くなった場合、移動体システム１００の走行経路を元に戻すことを決定しても良い。

また、制御内容決定手段１１３は、画像特徴点が所定の数より多いか否かで判断したが、これに限らず、位置合わせの指標として利用できるものがあればその指標を利用してもよい。例えば、画像データから検出したエッジ成分や平面成分、または輝度分布、あるいはマーカーのような模様を描いた領域を、指標として利用しても良い。

本実施形態において、特徴分布リストは要素が０個の初期状態としたが、これに限るものではない。特徴分布保持手段６０１が図７に示した処理の終了後も特徴分布リストを保持し、移動体システム１００が次回も同一の２次元平面上を走行すれば、他の方法でも良い。例えば、前回の処理において保持された特徴分布リストを読み込み、再利用しても良い。

［第４実施形態］
本実施形態では、本発明の方法を移動体における表示制御に適用した例を説明する。具体的には、算出した位置姿勢計測の信頼度の分布を画面上に表示する。

本実施形態における情報処理装置を備える移動体システムの構成例を示す図は、第２実施形態で説明した情報処理装置を備える移動体システムの構成例を示した図４と同じである。

本実施形態における情報処理装置１１０の処理手順を示すフローチャートは、第２実施形態で説明した情報処理装置１１０の処理手順を示すフローチャートである図５と同じである。以下では第２実施形態と異なるステップのみを説明する。

ステップＳ５０３では、制御内容決定手段１１３は、制御内容決定手段１１３が、ステップＳ５０２において保持した信頼度分布リストから定まる信頼度の分布を表示することを決定する。表示内容の詳細については後述する。

本実施形態において、制御内容決定手段１１３は、信頼度分布保持手段４１２が保持した位置姿勢と信頼度に対して、信頼度の高低に応じた領域を識別可能に表示することを決定する。

図８は、第４実施形態における表示画面を示す図である。具体的には、制御内容決定手段１１３が決定した表示内容に基づいて表示手段１０６が表示部１６に表示した画面の例を示す。

図８におけるマップ８００は、移動体システム１００が走行する空間を２次元平面で表したマップである。位置８０１は、移動体システム１００の現在位置である。経路８０２は、移動体システム１００が走行予定の経路である。凡例８０３は、位置姿勢計測の信頼度を色の濃さで示す凡例である。凡例８０３では、移動体システム１００が走行する空間領域の信頼度を色の濃さで表している。白い領域は、信頼度が０．８～１．０であり、位置姿勢計測の信頼度が高い領域を示す。薄いグレーの領域は、信頼度が０．５～０．８であり、位置姿勢計測の信頼度が比較的高い領域を示す。濃いグレーの領域は、信頼度が０．５未満であり、位置姿勢計測の信頼度が低い領域を示す。図８において、領域８０４は、信頼度が０．５未満であり、位置姿勢計測の信頼度が低い領域である。領域８０５は、信頼度が０．５～０．８であり、位置姿勢計測の信頼度が比較的高い領域である。

本実施形態の方法により、領域ごとの位置姿勢計測の信頼度の高低を可視化し、ユーザに表示する。これによりユーザが移動体システム１００の走行制御を変更することを促し、結果として移動体を安定して運行することができる。

制御内容決定手段１１３は、位置姿勢と信頼度に基づいて決定した移動内容を表示しても良い。例えば、図８の地点８０６は、位置姿勢と信頼度に基づいて決定した移動内容を反映する地点である。図８の説明８０７は、位置姿勢と信頼度に基づいて決定した移動内容に関する説明である。

また、移動体システム１００の姿勢変更の判断が可能となるように、制御内容決定手段１１３は、位置姿勢と信頼度に基づいて、所定の位置において方向毎の信頼度を表示するように決定しても良い。

第４実施形態の方法により、領域ごとの位置姿勢計測の信頼度の高低を可視化し、ユーザに表示する。これによりユーザが移動体システム１００の走行制御を変更することを促し、結果として移動体を安定して運行することができる。

［その他の実施形態］
以上、本発明をその好適な実施形態に基づいて詳述したが、本発明はこれら特定の実施形態に限られるものではなく、この発明の要旨を逸脱しない範囲の様々な形態も本発明に含まれる。また、上述の実施形態の一部を適宜組み合わせてもよい。

上述の実施形態では、移動体が２次元平面上を移動するとしたが、これに限らず、例えば、ドローンのように移動体が３次元空間上を移動しても良い。この場合、位置姿勢は３次元空間上の位置を示す３パラメータ（Ｘ、Ｙ、Ｚ）、および３次元空間上の姿勢を示す３パラメータ（ロール、ピッチ、ヨー）を合わせた６パラメータとなる。

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

１００…移動体システム
１０１…第１のセンサデータ取得手段
１０２…第２のセンサデータ取得手段
１０３…位置姿勢計測手段
１０４…通知手段
１０５…移動手段
１０６…表示手段
１１０…情報処理装置
１１１…依存度取得手段
１１２…信頼度算出手段
１１３…制御内容決定手段

Claims (14)

1. 第１のセンサデータを用いた第１の位置姿勢計測と第２のセンサデータを用いた第２の位置姿勢計測とに基づく位置姿勢計測の結果に基づいて移動体及び前記移動体の周辺機器の制御内容を決定する情報処理装置であって、
   前記位置姿勢計測における前記第２の位置姿勢計測に依存する度合いを示す依存度を取得する依存度取得手段と、
   前記依存度取得手段が取得した依存度に関する蓄積値の大きさに基づいて、前記位置姿勢計測の結果の信頼度を算出する信頼度算出手段と、
   前記信頼度算出手段が算出した前記信頼度に基づいて、前記制御内容を決定する制御内容決定手段と、
   を備え、
   前記位置姿勢計測は、前記第１の位置姿勢計測の精度が所定の閾値以上の場合は前記第２の位置姿勢計測を使用せず、前記第１の位置姿勢計測の精度が所定の閾値未満の場合は前記第２の位置姿勢計測を合わせて使用し、
   前記第２の位置姿勢計測は、前記第１の位置姿勢計測に比べて、時間または距離の経過に応じて蓄積する誤差がより大きく、
   前記依存度に関する蓄積値は、前記第２の位置姿勢計測を使い始めてから蓄積した値である、
   ことを特徴とする情報処理装置。
2. 前記信頼度算出手段は、前記依存度に関する蓄積値が大きくなる程、前記信頼度がより低くなるように前記信頼度を算出する、
   ことを特徴とする請求項１に記載の情報処理装置。
3. 前記信頼度算出手段は、前記依存度が所定の値を超えた状態が継続する時間が大きくなる程、前記信頼度がより低くなるように前記信頼度を算出する、
   ことを特徴とする請求項１に記載の情報処理装置。
4. 前記信頼度算出手段は、前記依存度が所定の値を超えた状態で移動する距離が大きくなる程、前記信頼度がより低くなるように前記信頼度を算出する、
   ことを特徴とする請求項１に記載の情報処理装置。
5. 前記制御内容決定手段は、前記信頼度に基づいて、前記信頼度の大きさを示す通知内容を決定する、
   ことを特徴とする請求項１乃至４のいずれか１項に記載の情報処理装置。
6. 前記制御内容決定手段は、前記信頼度に基づいて、前記移動体の進行、前記移動体の速度、前記移動体の姿勢、または前記移動体の経路、のいずれかを決定する、
   ことを特徴とする請求項１乃至４のいずれか１項に記載の情報処理装置。
7. 前記制御内容決定手段は、前記信頼度に基づいて、前記信頼度の大きさを識別可能な表示内容を決定する、
   ことを特徴とする請求項１乃至４のいずれか１項に記載の情報処理装置。
8. 前記位置姿勢計測の結果を取得する位置姿勢取得手段と、
   前記信頼度算出手段が算出した前記信頼度を、前記位置姿勢取得手段が取得した位置と関連して保持する信頼度分布保持手段と、
   をさらに備え、
   前記制御内容決定手段は、前記信頼度分布保持手段により保持された前記信頼度と前記位置に基づいて、前記第１の位置姿勢計測の精度が向上するように前記制御内容を決定する、
   ことを特徴とする請求項１乃至７のいずれか１項に記載の情報処理装置。
9. 前記位置姿勢計測の結果を取得する位置姿勢取得手段と、
   前記信頼度算出手段が算出した前記信頼度を、前記位置姿勢取得手段が取得した位置姿勢と関連して保持する信頼度分布保持手段と、
   をさらに備え、
   前記第１の位置姿勢計測の精度は、前記第１のセンサデータの計測時の前記移動体の姿勢に依存し、
   前記制御内容決定手段は、前記信頼度分布保持手段により保持された前記信頼度と前記位置姿勢に基づいて、前記第１の位置姿勢計測の精度が向上する方向に前記移動体の姿勢を変更するように前記制御内容を決定する、
   ことを特徴とする請求項１乃至７のいずれか１項に記載の情報処理装置。
10. 前記制御内容決定手段は、前記信頼度分布保持手段により保持された前記信頼度の分布に基づいて、前記第１の位置姿勢計測の精度が向上する経路に前記移動体の経路を変更するように前記制御内容を決定する、
    ことを特徴とする請求項８または９に記載の情報処理装置。
11. 前記制御内容決定手段は、前記信頼度分布保持手段により保持された前記信頼度の分布を表示するように制御内容を決定する、
    ことを特徴とする請求項８または９に記載の情報処理装置。
12. 前記第１のセンサデータは、画像データであり、
    前記第２のセンサデータは、慣性計測データである
    ことを特徴とする請求項１乃至１１のいずれか１項に記載の情報処理装置。
13. 第１のセンサデータを用いた第１の位置姿勢計測と第２のセンサデータを用いた第２の位置姿勢計測とに基づく位置姿勢計測の結果に基づいて移動体及び前記移動体の周辺機器の制御内容を決定する情報処理装置の制御方法であって、
    前記位置姿勢計測における前記第２の位置姿勢計測に依存する度合いを示す依存度を取得する依存度取得工程と、
    前記依存度取得工程において取得した依存度に関する蓄積値の大きさに基づいて、前記位置姿勢計測の結果の信頼度を算出する信頼度算出工程と、
    前記信頼度算出工程において算出した前記信頼度に基づいて、前記制御内容を決定する制御内容決定工程と、
    を有し、
    前記位置姿勢計測は、前記第１の位置姿勢計測の精度が所定の閾値以上の場合は前記第２の位置姿勢計測を使用せず、前記第１の位置姿勢計測の精度が所定の閾値未満の場合は前記第２の位置姿勢計測を合わせて使用し、
    前記第２の位置姿勢計測は、前記第１の位置姿勢計測に比べて、時間または距離の経過に応じて蓄積する誤差がより大きく、
    前記依存度に関する蓄積値は、前記第２の位置姿勢計測を使い始めてから蓄積した値である、
    ことを特徴とする情報処理装置の制御方法。
14. 請求項１乃至１２のいずれか１項に記載の情報処理装置の各手段としてコンピュータを機能させることを特徴とするプログラム。