JP2021099383A - 情報処理装置、情報処理方法およびプログラム - Google Patents

Abstract

【課題】 地図情報を更新された場合に、地図情報を使って位置推定する物体の自己位置姿勢が急激に変化することを抑制できる。【解決手段】 移動体が走行する環境で予め計測された特徴点の位置を含む地図情報と、前記環境を前記移動体が移動中に計測した特徴点を含むセンサ情報とに基づいて、前記地図情報に含まれる特徴点の新たな位置を取得する取得手段と、前記地図情報に含まれる特徴点の位置から推定される前記移動体の第１の位置と、前記取得された新たな特徴点の位置に基づいて推定される前記移動体の第２の位置と、の変化の大きさが閾値より大きい場合は前記地図情報の更新を抑制する更新手段と、を有することを特徴とする。【選択図】 図１

Description

本発明は、画像から地図情報を更新する技術に関する。

これまで、移動体を自立走行させる場合、移動体に搭載したセンサを用いて、センサの位置姿勢を算出するための地図を作成し、作成した地図を参照しセンサの位置姿勢を算出し、移動体を制御する技術があった。このとき、地図を高精度化するために複数時刻のセンサ値に基づいて地図を更新すること、地図が作成されていない領域で位置姿勢を算出するために時系列のセンサ値に基づいて地図を随時追加（更新）することがなされていた。特許文献１では、移動体が自律的に移動をしながら、地図を自動生成、更新する方法が開示されている。

特許第５４４４９５２号

しかし、特許文献１の技術では、例えば、新しい特徴点の位置についての情報が入る度に地図を更新すると、更新された地図によってセンサや移動体の位置姿勢の推定結果が変化する。特に、移動体の位置姿勢推定において元の推定結果と更新された推定結果の変化が大きい時、それに伴って移動体の移動速度や方向が急激に変化する可能性がある。

本発明は、上記の課題に鑑みてなされたものであり、地図情報を更新した場合に、地図情報を使って位置推定する物体の自己位置姿勢の推定結果が急激に変化することを抑制することを目的とする。

上記の目的を達成する本発明に係る情報処理装置は、移動体が走行する環境で予め計測された特徴点の位置を含む地図情報と、前記環境を前記移動体が移動中に計測した特徴点を含むセンサ情報とに基づいて、前記地図情報に含まれる特徴点の新たな位置を取得する取得手段と、
前記地図情報に含まれる特徴点の位置から推定される前記移動体の第１の位置と、前記取得された新たな特徴点の位置に基づいて推定される前記移動体の第２の位置と、の変化の大きさが閾値より大きい場合は前記地図情報の更新を抑制する更新手段と、
を有することを特徴とする。

本発明により、地図情報を更新した場合に、地図情報を使って位置推定する物体の自己位置姿勢の推定結果が急激に変化することを抑制できる。

情報処理装置の機能構成例を示すブロック図 情報処理装置が実行する処理の流れを示すフローチャート 情報処理装置のハードウェア構成例を示す図 ＧＵＩの一例を示す図 処理の詳細を示すフローチャート 処理の詳細を示すフローチャート 情報処理装置の機能構成例を示すブロック図 情報処理装置の機能構成例を示すブロック図 情報処理装置が実行する処理の流れを示すフローチャート 地図の更新前後における移動体の位置推定結果の一例を示す図 地図の更新を行うタイミングの一例を示す図

以下、図面を参照しながら実施形態を説明する。なお、以下の実施形態において示す構成は一例に過ぎず、本発明は図示された構成に限定されるものではない。

＜実施形態１＞
本実施形態では、情報処理システム１００に本発明における情報処理装置１００を適用した事例を説明する。情報処理システムとは、例えば、移動体と、センサ（計測装置）と、情報処理装置とから構成されるシステムである。移動体に搭載したセンサが計測したセンサ情報に基づきセンサの位置姿勢を算出し、算出した位置姿勢に基づいて移動体を目的地まで制御する制御値を算出し、制御装置１１が制御値に基づき移動体を目的地まで制御する。システムの構成はこれに限らず、例えば、制御値を生成するサーバと、移動体を観測する外界センサと、サーバと通信可能な移動体とから構成されるシステムでもよい。

本実施形態においては、センサ情報に基づき地図を作成・更新しつつ位置姿勢を算出する技術である、ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）技術を、センサの位置姿勢の計測に用いる。ＳＬＡＭ技術においては、地図を作成・更新すると、位置姿勢推定に用いる地図要素が変化するため、地図更新前後で算出する移動体の位置姿勢の推定結果が変化する。この変化により移動体の速度や方位が急激に変化することがある。

図１０を参照して上記のような例を説明する。１つめの例としては、観測視点が増えたことによって特徴点の位置が更新された場合である。例えば、移動体（カメラ）が移動すると、同じ特徴点を複数の視点で観測する。これにより、モーションステレオによる奥行精度が向上するため、より正確な特徴点の三次元位置が算出できる。算出した特徴点の位置を更新値として地図を書き換えると、位置姿勢に利用する特徴点の位置が変化（白点の１０００から黒点２０００に変化）することになり、カメラの位置姿勢の推定結果が１０１０から２０１０に変化する。２つめの例としては、ループクローズ発見により特徴点の位置が更新された場合である。ループクローズとは、例えば、移動体が周回するような走行環境を１周してスタート地点に戻ってきた際に、最初に推定したスタート地点と、戻ってきたときのスタート地点の推定位置を一致させる処理である。なお、同一地点であるか否かの判定は、はじめにスタート地点で撮像した画像と、スタート地点と同じ地点かその近傍に戻ってきたときに撮像した画像とを比較して、所定の値より画像特徴の類似度が大きくなった場合に同一地点であると判定する。このとき、同一地点判定を行った場合も、同一地点で撮像した画像に含まれる特徴点の世界座標が一致するように地図（特徴点）の三次元位置を再計算するため、使っている特徴点の三次元位置が変わることになりカメラの位置姿勢の推定結果も変化する。ループクローズ処理が必要な状況や、位置姿勢推定に用いる特徴点の増減や特徴点の位置の変化が起こる状況は、特徴点の位置推定誤差が所定の閾値より大きいまたは誤差が蓄積する場合や、環境が変化する場合に起こりうる。１つめの誤差による影響は、センサの計測精度や、推定処理を行う装置のパフォーマンスによって大きく変わる。また、環境の変化は、例えば、走行環境におけるレイアウト変更、人または物体が移動することによる変動、そして照明条件の変化などがある。このような環境変化が起こると、予め用意してあった地図にあった特徴点が増減することや、初めに観測した特徴点が動いてしまうことがある。そのため、地図を更新しながら自己位置姿勢推定を行うことが望ましい。

本実施形態においては、算出するセンサ１０の位置姿勢の、地図更新にともなう変化量が大きい領域、すなわち地図更新において要素を書き換える更新領域から所定の範囲に移動体が位置しているかどうかを判別する。例えば、座標が書き換わる特徴点を抽出して、それが囲む凸包領域を更新領域とする。そして、所定の範囲外に移動体が位置している場合に地図更新を実施する。地図の更新領域と移動体の位置との距離が離れているほうが、移動体の走行に地図の更新が影響しないからである。

本実施形態における情報処理システム１００は、図１に示すように、センサ１０、情報処理装置１、制御装置１１を有する。また、情報処理装置１は、更新情報入力部１１０、差分取得部１２０、更新方法決定部１３０から構成される。本実施形態の情報処理装置１は、さらにセンサ情報入力部１２、位置姿勢推定部１３、地図保持部１４、更新情報生成部１５、地図更新部１６、制御値決定部１７を有する。なお、センサ情報入力部１２、位置姿勢推定部１３、地図保持部１４、更新情報生成部１５、地図更新部１６、制御値決定部１７は情報処理装置の外部装置に含まれていても良い。

センサ１０は、外界の情報をセンサ情報として取得するセンサ（計測装置）である。本実施形態においては、センサ１０はステレオカメラであり、センサ情報としてステレオ画像を取得する。本実施形態におけるセンサ１０は、移動体に搭載されたカメラであるため、移動体が走行する環境を撮像する。

制御装置１１は、制御値決定部１７が算出した制御値に基づいて移動体を制御するための制御装置、すなわちアクチュエーターのことである。例えば、タイヤやステアリングを駆動するためのモーターや動力シリンダー、動力変換機のことである。本実施形態においては、制御装置１１は作動二輪型のタイヤのモーターのことである。制御値と、具体的には左右２個のタイヤの回転数のことである。制御値を変化させることで、移動体の直進、旋回動作を実現する。

センサ情報入力部１２は、センサ１０がセンサ（移動体）と周囲の環境との距離を計測した結果を、センサ情報として入力する。本実施形態では、センサ１０が取得した画像をセンサ情報として入力して、位置姿勢推定部１３に出力する。具体的には、移動体に搭載されたカメラによって移動体の周囲の環境を撮像した画像をセンサ情報とする。また、このセンサ情報から移動体（計測装置）と周囲の物体との相対的な距離を推定できるものとする。

位置姿勢推定部１３は、センサ情報入力部１２が入力したセンサ情報と、地図保持部１４が保持する地図情報と用いて、センサ１０の位置姿勢を算出する。算出した位置姿勢値を制御値決定部１７に出力する。本実施形態における地図情報とは、現実空間における特定の物体を示す１つまたは複数のキーフレーム情報を含むキーフレーム群情報を保持する。地図情報を用いて、センサ情報で得た特徴点の特徴と、予め計測した特徴点の特徴とを比較し、地図座標系におけるセンサの位置姿勢を計測できる。位置姿勢推定では、ＳＬＡＭ技術で一般的に用いられているように、センサで観測したセンサ情報から抽出した指標の座標と、地図情報に登録した指標の座標とが一致するようにセンサ位置姿勢を算出する。

制御値決定部１７は、位置姿勢推定部１３が算出した位置姿勢を入力し、制御装置１１を駆動する制御値を算出する。制御値とは、作動二輪タイヤの左右のタイヤそれぞれの回転数のことである。制御値決定部１７は、不図示の入力手段により入力した移動体の目標地点における移動体の位置姿勢と、位置姿勢推定部１７が算出した位置姿勢が一致するように、制御装置１１を制御するための制御値を算出し、制御装置１１に出力する。

地図保持部１４は、位置姿勢推定部１３がセンサ１０の位置姿勢を算出するために用いる地図情報を保持する。地図保持部１４は、地図情報を、更新情報生成部１５に出力する。情報処理装置１００の外部にあってもよい。ここで言う地図情報とは、環境の構造や空間を表現する環境の三次元モデルなどの三次元マップのことである。具体的には、三次元点群データ（もしくは三次元点群データと色情報との組み合わせ）、キーフレームデータの集合である。地図情報と、キーフレームについての詳細は後述する。

更新情報生成部１５は、地図保持手段１４が保持する地図情報に含まれる特徴点の位置と、センサ情報から新しく取得された特徴点の位置とに基づき、地図を更新するための更新情報を生成する。ここで、更新情報とは、地図情報に含まれる特徴点をセンサで観測したときに、地図情報における特徴点の位置とセンサ情報から取得された特徴点の位置との差分を示す情報であるとする。生成した地図の更新情報を、更新情報入力部１１０に出力する。地図情報を更新する方法は後述する。更新情報は、具体的には、以下のような情報を含む。１つめは、更新情報とは、所定の同一地点について、地図情報に含まれる既知の特徴点の位置と新たな特徴点の位置とに基づいて、マップ座標上で異なる位置を一致させるよう修正する情報である。同一地点は、２つの方法で算出したキーフレームＢの座標が同一座標値（計算誤差もあるので近い値）になるかどうかで判定する。例えば、世界座標上の他のキーフレームから見た所定のキーフレームと、世界座標上の所定のキーフレームとを比較して、差が所定の範囲内であるとき同一地点であると判定する。２つめは、更新情報とは、複数視点で観測した画像上の特徴点位置から求めた新たな特徴点座標を含む情報である。３つめは、更新情報とは、複数視点で観測した画像上の特徴点の位置から求めた特徴点の新たな位置と観測したカメラの新たな位置とを含む（キーフレームの座標）情報である。つまり、更新情報とは、センサで環境における特徴点を観測した際の特徴点の位置と、地図情報における特徴点の位置とを対応づけて、整合するように地図情報を更新するための情報である。

更新情報入力部１１０は、更新情報生成部１５が生成した、地図の更新情報を入力する。入力した更新情報を、差分取得部１２０に出力する。

差分取得部１２０は、センサ情報と地図情報とから推定される移動体の第１の位置と、センサ情報から新しく取得された特徴点の位置に基づいて推定される移動体の第２の位置と、の差分を取得する。センサ情報は移動体の周囲の環境を観測した特徴点の３次元における位置を示す。地図情報は、既に観測された特徴点の３次元の位置についての情報を含む。更新情報入力部１１０が入力した更新情報および地図保持部１４が保持する地図情報に基づいて、地図の更新に伴う位置姿勢推定部１３が算出するセンサ１０の位置姿勢への差分を算出する。差分とは、地図の更新前後での、位置姿勢推定部１３が算出する移動体の位置姿勢の変化の大きさに相当する値のことである。差分取得手段１２０は、算出した差分を更新方法決定部１３０に出力する。詳細な処理については後述する。

更新方法決定部１３０は、差分取得部１２０が算出した差分をもとに、地図情報の更新方法を決定する。本実施形態における更新方法とは、地図更新部１６が、地図更新を実行するか否かを判定するために用いる０〜１に正規化した値のことである。算出した更新方法を地図更新部１６に出力する。ここでは、値が大きい（１に近い）ほど、更新すると判断され、値が小さい（０に近い）ほど、更新を抑制する。

地図更新部１６は、更新方法に基づき、地図保持部１４が保持する地図情報を書き換える。更新情報から地図情報を更新する。更新方法に応じて、一度に更新してもよいし、複数回に分けて更新するようにしてもよい。また、取得された差分が小さい場合は、その段階では地図の更新を抑制する。

本実施形態における地図情報とは、１つまたは複数のキーフレーム情報を含むキーフレーム群情報を保持する。地図情報は、移動体を移動させる前に、センサを搭載した移動体を環境内で移動させることで生成する。例えば移動体をリモコンや手押しで操作しながら、移動体に搭載しているセンサによるセンサ情報を合成することでマップを生成する。あるいは、移動体が自律的に環境内を移動して、ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）技術を使って生成する。ＳＬＡＭとは、センサで周辺の環境を認識すると同時に、自分自身の位置や姿勢を精度よく推定する技術である。地図情報としてキーフレームデータを用いた場合は、まず、前フレームで推定した位置や姿勢に最も近いキーフレームを選択し、前フレームの位置や姿勢を用いてデプスマップに基づいて現フレームの各画素をキーフレームに射影する。次に、射影したフレームの画素値とキーフレームの画素値とをマッチングして輝度差が最小となるように位置姿勢を推定する。また、ループクローズした際など所定のタイミングでグラフ最適化法によりキーフレームの位置や姿勢を更新する。

キーフレームは、位置姿勢推定部１３がセンサの位置姿勢を算出するための最小単位の地図要素のことであって、ある地点でカメラが撮影した画像情報から取得した後述するキーフレーム情報をその時のカメラ位置姿勢と合わせて保持する。位置姿勢推定では、カメラが撮影した画像とキーフレーム情報とに基づいて、キーフレームに対するカメラに位置姿勢を算出し、さらにキーフレームの位置姿勢を掛け合わせることでセンサの位置姿勢を算出する。

キーフレーム情報とは、キーフレームのデータ構造のことである。キーフレーム情報は、環境内においてある視点から見た奥行情報を量子化して色の濃淡で表現したデプスマップを一定間隔で取得し、画像から取得した色情報を組合せたものである。なお、キーフレームは、位置姿勢を推定するためのものであれば何でも良く、例えば画像中の特徴点における奥行き情報と画像情報（色情報）との組み合わせであっても良い。キーフレームを一意に識別するためのＩＤ、センサ１０が撮像した画像情報、画像から検出した角といった特徴点情報、およびキーフレームの位置姿勢値すなわち画像を撮影した時点のセンサの位置姿勢を表す行列を保持する。特徴点情報は、画像中で検出した特徴点の二次元座標ｕ、ｖ（以降特徴点の二次元位置ないし特徴点の位置と表記する）、特徴量（画像周囲の小パッチを切り出した小画像）、空間中の三次元位置を示す三次元座標Ｘ、Ｙ、Ｚを保持する。位置姿勢値は、現実空間中に規定された世界座標における撮像装置の位置を表す３パラメータと、撮像装置の姿勢を表す３パラメータとを合わせた６パラメータを４×４行列Ｍとして表記した行列値のことである。

本実施形態における地図の更新情報とは、地図情報が保持するキーフレーム群情報の中の更新対象となるキーフレーム情報の更新値（キーフレーム更新情報）のことである。キーフレーム更新情報は、キーフレーム群に含まれる更新対象の各々のキーフレームを識別するためのＩＤ、更新対象のキーフレームの位置姿勢値の更新後の値、および更新対象のキーフレーム情報に含まれる特徴点の三次元位置の更新後の値を含む。また、複数のキーフレーム更新情報をまとめて、キーフレーム群更新情報と呼ぶ。キーフレーム群更新情報は、キーフレーム更新情報を含む。

図３は、情報処理装置１のハードウェア構成を示す図である。Ｈ１１はＣＰＵであり、システムバスＨ２１に接続された各種デバイスの制御を行う。Ｈ１２はＲＯＭであり、ＢＩＯＳのプログラムやブートプログラムを記憶する。Ｈ１３はＲＡＭであり、ＣＰＵであるＨ１１の主記憶装置として使用される。Ｈ１４は外部メモリであり、情報処理装置１が処理するプログラムを格納する。入力部Ｈ１５はキーボードやマウス、ボタン、スイッチであり、情報等の入力に係る処理を行う。表示部Ｈ１６はＨ１１からの指示に従って演算結果を表示装置に出力する。なお、表示装置は液晶表示装置やプロジェクタ、ＬＥＤインジケーターなど、種類は問わない。Ｈ１７は通信インターフェイスであり、ネットワークを介して情報通信を行うものであり、通信インターフェイスはイーサネット（登録商標）でもよく、ＵＳＢやシリアル通信、無線通信等種類は問わない。本実施形態においては、移動体の目標位置の入力は通信インターフェイスＨ１７を介して行うものとする。Ｉ／Ｏである。本実施形態においては更新情報入力部１１０が入力する更新情報を更新情報生成部１５から入力する、更新方法決定部１３０が算出した更新方法を地図更新部１６に出力する際にＩ／Ｏを介して行う。

次に、本実施形態における処理手順について説明する。図２は、本実施形態における情報処理装置１を含む情報処理システムの処理手順を示すフローチャートである。処理ステップは、初期化Ｓ１１、センサ情報入力Ｓ１２、位置姿勢推定Ｓ１３、制御Ｓ１４，地図更新判定Ｓ１５、更新情報生成Ｓ１５，地図更新Ｓ１６、終了判定Ｓ１７、更新情報入力Ｓ１１０、差分取得Ｓ１２０、更新方法決定Ｓ１３０を行う。

Ｓ１１では、情報処理装置１が、システムの初期化を行う。すなわち、外部メモリＨ１４からプログラムや地図情報をＲＡＭ（Ｈ１３）に読み込む。また、位置姿勢推定部１３が位置を推定するために必要な次の値を取得する。センサ１１（カメラ）の内部パラメータ（焦点距離ｆ_ｘ（画像の水平方向）、ｆ_ｙ（画像の垂直方向）、画像中心位置ｃ_ｘ（画像の水平方向）、ｃ_ｙ（画像の垂直方向）、レンズ歪みパラメータ）を取得する。また、制御装置のパラメータ（移動体大きさやタイヤの直径、モーターのトルク値等）等の各種パラメータを外部メモリＨ１４からＲＡＭ（Ｈ１３）に読み込む。また、センサ１０や制御装置１１を起動し、情報処理装置１を含む情報処理システムが動作・制御可能な状態とする。初期化完了後、センサ情報入力Ｓ１２に移る。

Ｓ１２は、センサ情報入力部１２が、計測装置（移動体）の周囲を計測したセンサ情報を入力する。ここでは、センサ１０であるカメラが撮像した画像を入力する。入力した画像は、内部パラメータを用いてレンズ歪みの影響を除去した補正画像に変換する。補正画像を位置姿勢推定部１３に出力する。以降、補正画像は単に画像と呼ぶ。次に、位置姿勢推定Ｓ１３に移る。

Ｓ１３では、位置姿勢推定部１３が、計測装置の周囲を計測したセンサ情報から取得した特徴点の位置と、環境に存在する特徴点の位置を含む地図情報とに基づいて、移動体の位置及び姿勢（第１の位置姿勢）を推定する。すなわち、センサ情報入力部１２が入力した画像および地図保持部１４が保持する地図情報を用いて、センサ１０の位置姿勢を算出する。センサ１０の位置姿勢推定には、Ｒａｕｌらの方法を用いる（Ｒａｕｌ Ｍｕｒ−Ａｒｔａｌ ｅｔ．ａｌ， ＯＲＢ−ＳＬＡＭ： Ａ Ｖｅｒｓａｔｉｌｅ ａｎｄ Ａｃｃｕｒａｔｅ Ｍｏｎｏｃｕｌａｒ ＳＬＡＭ Ｓｙｓｔｅｍ． ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｒｏｂｏｔｉｃｓ）。まず、センサ情報である画像から検出した特徴点を、キーフレーム情報に含まれる特徴点とマッチングする。次に、マッチングした特徴点の３次元座標、と検出した特徴点の画像上での二次元位置の対応関係から、ＰＮＰ問題（Ｐｅｒｓｐｅｃｔｉｖｅ−ｎ−Ｐｏｎｔ Ｐｒｏｂｌｅｍ）を解きセンサの位置姿勢を算出する。算出した位置姿勢を、制御部１７に出力し、Ｓ１４に移る。

Ｓ１４では、制御値決定部１７が、移動体の位置姿勢に基づいて、予め設定された目標地点に移動体が近づくような制御値を決定する。すなわち、移動体の目標地点（または経路）の位置姿勢を示す地図情報と、位置姿勢推定部１３が推定したセンサ１０の位置姿勢と、に基づいて移動体を移動させる。制御値の算出には、移動体の位置姿勢と、地図情報における目標地点の位置姿勢との両者のユークリッド距離を小さくするような制御値の取りうる全てのバリエーションを制御値の候補として算出する。候補の中から最適な制御値を選択するＤｙｎａｍｉｃ Ｗｉｎｄｏｗ Ａｐｐｒｏａｃｈ（ＤＷＡ）を用いる。算出した制御値を、制御装置１１に出力する。制御値を出力後、地図更新Ｓ１５に移る。

Ｓ１５では、更新情報生成部１５が、更新情報を生成する。更新情報生成には、Ｒａｕｌらの方法に記載がある、ループ検出およびポーズグラフ最適化を用いる（Ｒａｕｌ Ｍｕｒ−Ａｒｔａｌ ｅｔ．ａｌ， ＯＲＢ−ＳＬＡＭ： Ａ Ｖｅｒｓａｔｉｌｅ ａｎｄ Ａｃｃｕｒａｔｅ Ｍｏｎｏｃｕｌａｒ ＳＬＡＭ Ｓｙｓｔｅｍ． ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｒｏｂｏｔｉｃｓ）。まず、地図情報に含まれるキーフレーム群情報の中から、物理的に同一地点に位置しているのに、異なる位置姿勢として算出されている２つのキーフレーム情報を選択するループ検出を行う。次に、ポーズグラフ最適化により、それら２つのキーフレーム情報が保持する位置姿勢が整合するように周囲のキーフレーム情報の位置姿勢値の更新値、および特徴点の三次元座標の更新値を算出する。更に具体的に手順を説明する。まず、キーフレームＡとキーフレームＢの特徴点をマッチングする。マッチングしたキーフレームＡの特徴点の３Ｄ座標とキーフレームＢの画像上の特徴点の２Ｄ座標を使って３Ｄ−２Ｄの位置姿勢算出を行う（これにより、キーフレームＡから見たキーフレームＢの座標がわかる）。算出したＡ→Ｂの位置姿勢を使ってキーフレームＡの特徴点の３Ｄ座標→キーフレームＢへの２Ｄ投影処理を行って、所定の場所に投影されるかチェックする。キーフレームＡから見たキーフレームＢの座標と、キーフレームＢが保持する座標とが一致するように全体最適化を行う。このようにして算出した複数のキーフレーム更新情報を、キーフレーム群更新情報すなわち更新情報として、更新情報入力部１１０に入力し、更新情報入力Ｓ１１０に移る。

Ｓ１１０は、更新情報入力部１２０が、更新情報生成部１５が生成した更新情報を、差分取得部１２０に入力し、Ｓ１２０に移る。

Ｓ１２０は、差分取得部１２０が、元の地図情報におけるセンサの位置と、更新情報におけるセンサの位置と、の変化と、更新情報とを用いて、位置姿勢推定部１３が推定する位置姿勢の地図更新前後の変化の大きさを示す差分を取得する。まず、更新対象のキーフレームとセンサとの距離を算出する。具体的には、キーフレーム群更新情報中の各キーフレーム更新情報に含まれる更新対象のキーフレームの位置の更新値とセンサの位置とに基づいて、更新対象の各キーフレームの座標とセンサの座標との三次元のユークリッド距離を取得する。次に、更新対象のキーフレームごとに算出したユークリッド距離の最小値を求める。本実施形態においては、最小ユークリッド距離の逆数を差分値として利用する。すなわち、距離が大きい（位置姿勢計測に影響のない）程、地図を更新しやすくする。位置姿勢計測に算出した差分値を更新方法決定部１３０に出力し、Ｓ１３０に移る。

Ｓ１３０は、更新方法決定部１３０が、差分取得部１２０が算出した差分に基づき、地図更新手段１６が地図を実行するかどうかの判定に用いる更新方法を決定する。更新方法のひとつとして、更新しやすさを示す値を０〜１に正規化した実数とし、差分の値が小さいほど０に、差分の値が大きいほど１近づくように構成する。本実施形態においては、正規化には、Ｓ１２０において差分取得部１２０が算出した差分値に−１をかけた値を底とする指数関数を用いる。このようにして決定した更新しやすさを示す値に基づいて、更新しやすさを示す値が所定の閾値以上であれば、更新する方法を決定する。更新する場合の更新方法を地図更新部１６に出力し、Ｓ１７に移る。更新しやすさを示す値または更新情報に応じて、更新方法を変えるように設定しておいても良い。更新しやすさを示す値が閾値未満であれば地図を更新せず、更新しないことを示す情報を地図更新部に入力する。

Ｓ１６では、Ｓ１３０において更新方法決定部１３０が決定した更新方法に基づいて、地図更新部１６が、更新方法に基づいて、地図保持部１４が保持する地図情報を更新するか否か判定し、地図を更新する。本実施形態においては、地図の更新情報に基づいて地図保持部１４が保持する地図情報を書き換え、Ｓ１７に移る。更新しやすさを示す値が閾値未満の場合は、その段階では地図更新を抑制し、次にＳ１７に移る。その場合は、例えば、所定時間後に更新することや、更新方法に基づいて段階的に複数回に分けて地図を更新する。例えば、地図更新領域とセンサが遠い時には地図更新を行い、地図更新領域とセンサが近い時には地図更新をやめることになる。

Ｓ１７では、システムを終了するか否かの判断を行う。マウスやキーボードなど入力部Ｈ１５を介して利用者からシステムを終了するコマンドが入力されている場合にはシステムを終了し、そうでなければＳ１２に戻り位置姿勢推定および地図更新処理を継続する。

＜効果＞
実施形態１では、更新領域からセンサ、すなわち移動体が離れたときに地図更新を許容することで、更新における位置計測値の変化量を小さくすることができる。これにより、移動体の急激な速度変化や方向変化が少なくなる。

＜変形例１＞
本実施形態においては、差分取得部１２０が算出する差分とは、更新対象の個々のキーフレームとセンサとの距離に基づいて算出していた。しかしながら、実施形態は上記に限るものではない。キーフレーム更新情報に基づいて算出し、かつ前述の位置姿勢算部１３が算出するセンサ１０の、地図更新に伴う位置姿勢の変化量が小さくなるような差分の値を算出するものであればよい。例えば、差分として、更新対象の個々のキーフレームとセンサとの姿勢の差を用いてもよい。姿勢の差とは、キーフレーム群更新情報中の各キーフレーム更新情報に含まれる更新対象のキーフレームの姿勢の更新値と、位置姿勢推定部１３が算出したセンサの姿勢値の角度差である。具体的には、移動体が周回していたルートを逆走するときや、往復して戻ってくるときに、進行方向に応じて地図の更新を切り替えることができる。また、前述の更新対象のキーフレーム群に外接する凸包空間を算出し、凸包空間の表面とセンサとの距離に基づいて差分として算出してもよい。さらに言えば、差分取得部１２０が、凸包空間内に前記センサが位置するかの二値の値を算出するようにしてもよい。すなわち、凸包内にセンサが位置していれば差分１、凸包にセンサが位置していなければ差分０として差分を決定する。

本実施形態においては、センサと周囲の物体との距離の逆数を差分として算出していた。距離が増大するほど、差分の値が減少するような設定方法であれば距離の逆数に限らず何でもよい。すなわち、センサと周囲の物体との距離から差分を算出するには、逆余接関数としてもよいし、底を距離値の負値とした指数関数を用いてもよいし、シグモイド関数に距離値を代入し、−１をかけ、１を足した関数を用いてもよい。

また、キーフレーム群更新情報の中の個々のキーフレーム更新情報に含まれる特徴点の三次元座標とセンサ１０との距離に基づいて差分を算出してもよい。このような構成とする場合には、キーフレーム群更新情報に含まれる特徴点情報の中でセンサ１０とユークリッド距離が最小となる特徴点情報を求める。この時、距離最小となる特徴点との距離値の逆数を、差分の値として算出する。距離最小となる特徴点に限らず、更新範囲からの距離を測れるものであればよい。すなわち、特徴点のうちセンサとの距離が小さいものから所定数選び出し、それら特徴点とセンサとの距離の平均値や中央値を用いてもよい。また、センサとの所定距離以下の特徴点数を差分の値として算出してもよい。

更新情報が含む範囲の広さ、すなわちキーフレーム群が含まれる領域の広さに基づいて差分を算出することもできる。すなわち、更新領域が広いほど大きな差分を算出する。具体的には、前述の更新対象のキーフレーム群の凸包の体積を算出し、体積値の逆数を差分の値として算出する。また、差分の値は、凸包空間の表面積の逆数の値でもよい。凸包空間に外接や内接する楕円の体積や長軸の長さの逆数を用いてもよい。凸包はキーフレーム群が含まれるよう算出したが、更新情報に含まれる特徴点を含む凸法でもよい。

更新方法とは、地図更新のタイミングを変えることがわかるものであれば本実施形態に示した方法に限らない。例えば、更新対象のキーフレームごとに差分を算出し、更新対象のキーフレームごとに更新方法を算出する構成としてもよい。このような構成とすると、キーフレームごとに、更新をコントロールすることになり、差分が小さいキーフレームは早く更新し、差分が大きなキーフレームは更新しない。このようにすることで、地図は着々と更新しつつ、算出する位置姿勢の急激な変動を抑えることができる。

地図更新部１６は、更新前後の移動体の推定位置の差分が大きいほど時間をかけて徐々に、キーフレームや特徴点の位置姿勢が更新後の値となるように更新することもできる。具体的には、キーフレーム群更新情報に含まれる更新対象のキーフレームの位置姿勢と、地図保持部１４が保持する当該キーフレームの位置姿勢を重み付き和で合成した合成位置姿勢を用いて地図保持部が保持するキーフレームを更新する。図１１のように、地図の更新方法（ここでは特徴点の更新前後での位置の変化）が大きい場合には、地図を更新する量を所定の閾値に抑え、複数回に分けて徐々に特徴点位置を補正することで、急激にカメラ位置が変化することを防ぐ。特に地図の更新方法が大きいほど、補正回数を増やすことで、より緩やかにカメラの位置の変化を起こすことができる。キーフレーム群を含むキーフレームの位置姿勢にかかる重みを、位置姿勢を計測するたびに増大させる。このとき、増大割合は更新方法が大きいほど小さな値となるようにする。このようにすることで、位置姿勢推定への影響が大きな時ほどゆっくり地図を更新することで、更新における位置計測値の変化量が減少し、移動体の急激な速度変化や方向変化が少なくなる。

本実施形態においては、地図の更新情報とは、地図の構成要素の値の更新、すなわちキーフレームの位置姿勢や特徴点の三次元位置の更新のことであった。本実施形態における地図の更新方法は、地図の切り替え時すなわち地図情報に含まれる一部のキーフレーム群を、不図示の保持手段が保持する新たなキーフレーム群に置き換える場合に用いることもできる。このような構成の場合には、差分取得部１２０は、新たなキーフレーム群情報をキーフレーム群更新情報として差分を算出する。このようにすることで、地図の切り替え時にも算出する位置計測値の変化量が減少し、移動体の急激な速度変化や方向変化が少なくなる。

さらに言えば、地図の切り替えに限らず、不図示の保持手段が持つグローバルな地図を、保持手段１４に一部ローカルな地図としてロードするときにも適用できる。このようにすると、例えば、別の移動体が作成した地図をロードして、自身の地図を更新（保持手段１４が保持する地図に追加）する場合、移動体の速度や方位が急激に変化することが少なくなる。同様に、例えば、クラウドサーバー上にある地図の一部をローカルマップとしてロードして位置姿勢を推定する際に、地図をロード（保持手段１４が保持する地図に追加）した時の移動体の速度や方位が急激に変化することが少なくなる。

本実施形態においては、地図情報とはキーフレーム群のことであった。しかしながら地図情報はこれに限らず、センサの位置姿勢を算出するために用いる指標であればなんでもよい。例えば、位置姿勢推定に物体を指標として位置計測する、すなわちカメラで撮影した物体の三次元座標に基づいて位置姿勢を算出する構成とした場合には、物体とその三次元座標が地図情報となる。このような構成とした場合には、物体の位置姿勢の更新値を本実施形態におけるキーフレーム更新情報として扱うことで、本実施形態の方法で差分を算出することができる。

また、地図情報はキーフレームを保持しない構成、すなわち特徴点のみ保持する構成としてもよい。このような場合にも、特徴点の三次元位置の更新値を本実施形態におけるキーフレーム更新情報として用いることで、本実施形態における差分を算出することができる。

また、センサとしてＬｉＤＡＲを用いる場合には、ＬｉＤＡＲが計測したスキャン点群や、Ｏｃｃｕｐａｎｃｙ Ｇｒｉｄ Ｍａｐ（通行可能領域と通行付加領域すなわち壁等の領域とを格子にプロットしたデータ構造である地図）を用いることもできる。このようなＬｉＤＡＲを用いる構成の場合には、各グリッドを本実施形態で説明した特徴点とみなせば、本発明を適用できる。具体的には、各グリッドからの距離値に基づいて本実施形態で用いた方法で差分を算出する。この方法に限らず、更新対象のグリッドを更新領域とみなせば、更新領域からのセンサの距離を求めることができる。また、センサが位置しているグリッドが更新対象か否かにより、更新領域内にセンサが位置しているか否かを判定することもできる。このような構成として、ＬｉＤＡＲを用いる場合にも本発明を適用できる。

本実施形態においては、更新情報とは、キーフレーム群更新情報が含むキーフレーム更新情報であり、更新対象のキーフレームの更新後の値や特徴点の更新後の値であった。更新情報はこれに限らず、地図を更新することができる情報であればよい。すなわち、地図保持手段１１０が保持する地図の要素の修正値（差分量）でもよい。

また、本実施形態では、更新情報はキーフレーム更新情報として保持する構成を説明したが、地図保持手段１１０が保持する地図の要素を書き換えるための更新値を保持する構成であればキーフレーム更新情報として保持する構成に限らない。すなわち、キーフレームを識別するためのＩＤと更新後の位置姿勢や特徴点の三次元位置のみの値を保持する構成とすることもできる。また、変形例で説明したＯｃｃｕｐａｎｃｙ Ｇｒｉｄ Ｍａｐを地図情報として用いる場合には、セルの座標と書き換え値を更新情報として保持する構成としてもよい。

本実施形態においては、更新情報生成には、ループを検出し、ポーズグラフ最適化により、更新対象のキーフレームの位置姿勢の更新値と特徴点の三次元位置の更新値を求めていた。一方、更新情報の生成は、上記の方法に限らない。例えば、ポーズグラフ最適化の代わりに、Ｒａｕｌらの方法に記載があるバンドル調整を用いることもできる（Ｒａｕｌ Ｍｕｒ−Ａｒｔａｌ ｅｔ．ａｌ， ＯＲＢ−ＳＬＡＭ： Ａ Ｖｅｒｓａｔｉｌｅ ａｎｄ Ａｃｃｕｒａｔｅ Ｍｏｎｏｃｕｌａｒ ＳＬＡＭ Ｓｙｓｔｅｍ． ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｒｏｂｏｔｉｃｓ）。ＩＭＵ（ｉｎｅｒｔｉａｌ ｍｅａｓｕｒｅｍｅｎｔ ｕｎｉｔ）や、移動体のタイヤの回転量の積分値から求めたオドメトリの計測値との整合性を取るためのカルマンフィルタにより算出した位置姿勢に基づきキーフレームの位置姿勢の更新値を求めてもよい。

さらには、地図の追加すなわち地図情報が保持するキーフレーム群情報に新たなキーフレーム情報を追加する場合にも、本発明を適用することができる。このような場合には、追加する新たなキーフレーム情報をキーフレーム更新情報として用いることで、本実施形態の方法を適用できる。

本実施形態では、センサ１０はカメラのことであった。カメラとしてはモノクロカメラ、カラーカメラ、ステレオカメラ、奥行カメラなど、位置姿勢計測に用いることができる画像を取得できるカメラであればなんでもよい。カメラに応じて、センサ情報はモノクロ画像、ＲＧＢ画像、デプスマップなど位置姿勢推定ができるものであれば何でもよい。

また、センサ１０はＬｉＤＡＲを始めとする、位置計測に用いることができるセンサ情報を取得できるものであればなんでもよい。例えば、２Ｄ ＬｉＤＡＲでも、３Ｄ ＬｉＤＡＲでもよい。センサの種類に応じて、センサ情報は二次元の点群でもよいし三次元の点群でも位置姿勢推定ができるものであれば何でもよい。

センサ１０は上記に限らず、地図情報とセンサ１０が計測したセンサ値に基づいて位置姿勢を算出することができるセンサであればなんでもよい。

対象とする制御装置１１は、タイヤに限らず、ステアリングや、移動体がドローンであればプロペラの回転量など、移動体の速度・加速度・角度・角速度、角加速度を変化させるものであればなんでもよい。

本情報処理装置は、更新した地図を用いて位置姿勢を算出するすべての構成に適用できる。例えば、移動体は、ＡＧＶ／ＡＭＲ、無人搬送車、自動運転車、ドローン、自律移動ロボットであっても良く、本実施形態で説明した移動制御をそれらに適用しても良い。また、陸上を歩行・走行する移動体以外でも、空中を飛行して移動するものや、水上を移動するもの、水中を潜水して移動する移動体に適用しても良い。

実施形態においては、情報処理装置１は、情報処理システムに搭載する構成として説明した。しかしながら、地図の更新方法を算出することができる構成であれば、情報処理装置１はどこに配置してもよい。例えば、情報処理システムとは分離して、クラウドサーバーに配置してもよいし、別の情報処理システム上に配置することもできる。動作タイミングも任意である。本実施形態においては処理ステップも逐次処理を説明したが、特定の処理として例えばＳ１５からＳ１６をＳ１２からＳ１４と別のスレッドとして並列して動作させるような構成としてもよい。

本実施形態においては、画像が入力される毎に地図更新判定を行う構成について説明した。しかしながら、画像が入力される毎に地図更新判定を行う構成に限らず、所定のタイミングで地図更新判定を行う構成としてもよい。

すなわち、更新情報生成ステップＳ１５はキーフレームを追加するごとにステップＳ１１０以降を実行する構成としてもよい。さらには、新たに追加したキーフレームと地図保持部１４が保持するキーフレームとをマッチングしループ検出したときのみステップＳ１１０以降を実行する構成としてもよい。このようにすると、地図を更新しうるときのみにステップＳ１１０以降を実行することになり、処理負荷を低減し、地図更新判定をすることができる。

＜変形例２＞
情報処理装置１に、不図示の提示部を含む構成としてもよい。例えば、提示部に赤、黄色、緑の三色灯を用いるのであれば、地図更新をしていないときに緑色、地図更新中は黄色となるように提示し、利用者に地図更新中であることを可視化することができる。表示の色は利用者に地図更新中であることを可視化できれば任意である。地図更新ステータスに関する情報を利用者に提示できるものであれば、ＬＥＤランプでも、液晶ディスプレイでもなんでもよい。提示装置はスピーカーでもよく、地図更新ステータスに応じて特定のアラーム音や特定のメロディーが流れるように構成としてもよい。

提示部に、地図更新に関わる情報として、差分や更新方法、更新範囲を提示してもよい。図４は、提示部にディスプレイを用い、地図情報と差分、更新範囲を提示するＧＵＩ１００の事例である。

Ｇ１１０は地図情報と差分を提示する提示領域である。Ｇ１１１は、地図更新を行う範囲、および差分の値を示す指標である。本変形例においては、差分が大きいほど濃い色となるように地図更新領域を提示する。Ｇ１１２は移動体の移動ルートである。Ｇ１１３は移動体の現在位置である。

Ｇ１２０は、地図更新のパラメータを利用者が調節するためのＧＵＩである。Ｇ１２１はマウスカーソルであり、Ｇ１２２に示した閾値調整バーを上下に移動させ、差分がどの程度であれば地図更新を許容するのか決めることができる。Ｇ１２３は、差分の大小を表す指標である。

このようにして、利用者により容易に地図更新が行われる領域を提示し、地図更新を許容する閾値を調節することができる。このようにすることで、利用者に地図更新による差分を直感的に理解することができる。

なお、提示部にディスプレイを用いる構成について説明したが、提示装置はスピーカーでもよい。移動体が通過している領域の差分に応じてアラーム音の大小や高低、メロディーの種別を変えるよう構成することもできる。

＜実施形態２＞
実施形態１では、差分取得手段１２０が、更新領域との距離を用いて差分取得を行っていた。実施形態２では、更新要素の変化量に基づき、差分を算出する。本実施形態においては、変化量とはキーフレーム群更新情報に含まれる更新対象のキーフレームの更新に伴う位置の変化の大小のことである。キーフレームの位置の変化が大きいほど、算出するセンサの位置姿勢の変化量が大きくなるため、キーフレームの位置の変化量が大きいほど大きな差分を算出する構成について述べる。

実施形態２における装置の構成は、実施形態１で説明した情報処理装置１の構成を示す図１に加え、差分取得部１２０が、地図保持部１４の地図情報を入力する点が異なる。ハードウェア構成は実施形態１と同様に図３のような構成を用いる。

実施形態２における処理手順は、実施形態１で説明した情報処理装置１の処理手順を示す図２と同一であるため、説明を省略する。実施形態１とは、差分の算出Ｓ１２０の処理の詳細が異なる。

更新方法決定Ｓ１３０は、キーフレーム群更新情報に含まれるキーフレーム更新情報が含む、更新対象のキーフレームの位置の変化量を算出する。本実施形態における位置の変化の大小とは、更新前のキーフレームの座標と、更新後のキーフレームの座標とのユークリッド距離のことである。キーフレーム群更新情報に含まれる各々の更新対象のキーフレームに対して算出した距離値の最大値を差分として更新方法決定部１３０に出力し、Ｓ１７に移る。

＜効果＞
実施形態２では、更新要素の変化量に基づいて、地図の更新方法を算出する。すなわち、地図更新要素の変化が所定値以下であれば更新し、更新方法が所定値以上であれば更新しない。このような構成にすることで、更新における位置計測値の変化量が所定以上にならないようにする。このようにすることで、移動体の急激な速度変化や方向変化が少なくなる。

＜変形例＞
実施形態２では、差分とは、地図更新前後のキーフレームの座標のユークリッド距離に基づいて算出していた。しかしながら、地図の更新要素の変化量を基に算出するものであればなんでもよい。すなわち、キーフレームの姿勢の変化量でもよい。また、特徴点の三次元位置の変化量でもよい。さらには、変化量とは地図の構成要素の位置や姿勢の幾何学的な変化に限らず、位置姿勢計測に用いる地図要素の変化量であればなんでもよい。具体的には、特徴点が平滑化画像の局所領域内の勾配方向のヒストグラムを特徴量とするＳＩＦＴやＯＲＢのよう特徴量を保持しているときには、特徴量の更新量であってもよい。特徴量の更新とは、複数のキーフレーム情報に含まれる特徴点情報のうち、三次元座標と特徴量が類似しており同一とみなした特徴点の特徴量の、平均化や代表値への置き換え処理のことである。特徴量の更新量は、例えばＯＲＢやＳＩＦＴの更新前後の特徴量ベクトルのノルムやコサイン類似度の値によって求める。Ｄｅｓｃｒｉｐｔｏｒに限らず、特徴点の小領域のパッチを複数視点で平滑化して更新する場合は、パッチの更新前後の輝度の変化の合計や平均、最大値等の値を用いてもよい。

また、位置姿勢推定装置１６が、センサの位置姿勢推定に用いている地図指標のうち、変化のある指標の割合を変化量としてもよい。例えば、位置姿勢推定に用いている特徴点のうち、地図更新の前後で三次元位置が所定の距離以上移動する特徴点の割合を変化量とする。また、位置の変化だけでなく、地図要素の増減を変化量として用いてもよい。すなわち、位置姿勢推定に用いている特徴点数の増減でもよい。さらに言えば、参照しているキーフレームの増減数や、それらの割合を用いてもよい。

センサ１０にＬｉＤＡＲを用いる場合は、Ｏｃｃｕｐａｎｃｙ Ｇｒｉｄ Ｍａｐの特定の格子要素が、地図更新によりどの格子へ移動するか算出し、更新前後の二つの格子のユークリッド値を差分として用いることもできる。さらに言えば、Ｏｃｃｕｐａｎｃｙ Ｇｒｉｄ Ｍａｐの、地図更新に伴う各格子の値の変化量を差分値として用いてもよい。ここでいう各格子の値とは、Ｏｃｃｕｐａｎｃｙ Ｇｒｉｄ Ｍａｐにおける、物体が存在しているか、していないかを表す０〜１の確率値のことである。すなわち、差分とは、特定の格子の確率値の地図更新前後での変化値を用いてもよい。

＜実施形態３＞
実施形態２では、更新に伴う地図の要素の変化量を基に差分を算出していた。実施形態３では、地図の要素ではなく、位置姿勢推定部１３が地図更新前後で算出するセンサ１０の位置姿勢の変化量に基づいて、差分を算出する。すなわち、地図更新に伴って、算出される位置姿勢の変化量が大きくなるほど、増大するように差分を算出する方法について述べる。

実施形態３における装置の構成、および処理ステップは、実施形態１で説明した情報処理装置１の構成を示す図１と同一であるため省略する。また、ハードウェア構成は実施形態１と同様に図３のような構成を用いる。実施形態と異なるのは、差分取得手段１２０が差分を算出する処理ステップである。

実施形態３における処理全体の手順は、実施形態１で説明した情報処理装置１の処理手順を示す図２と同一であるため、説明を省略する。実施形態１とは、差分取得Ｓ１２０において、差分取得部１２０が、地図更新後に位置姿勢推定部１３が算出するセンサ１０の位置姿勢を推定する点が異なる。

図５は、本実施形態３における差分取得部１２０の処理Ｓ１２０の詳細を示した図である。Ｓ１２１０では、差分取得部１２０が、センサ１０の周辺の地図要素を選択する。本実施形態においては、位置姿勢推定部１３が算出した位置姿勢に基づき、地図保持手段１１０が保持するキーフレームの中から、センサ１０と所定以内の距離にあるキーフレーム情報を所定数選択し、Ｓ１３２０に移る。選択したキーフレーム情報のことを、本実施形態においては近傍キーフレーム情報と呼ぶ。

Ｓ１２２０では、差分取得部１２０が、更新情報入力部１１０が入力したキーフレーム群更新情報のキーフレーム更新情報の中から、近傍キーフレーム情報の各キーフレーム情報に含まれるＩＤと一致するキーフレーム情報を選択する。次にＳ１２２０に移る。なお、選択したキーフレーム更新情報を近傍キーフレーム更新情報と呼ぶ。

Ｓ１２３０では、差分取得手段１３０が、Ｓ１２２０で取得した近傍キーフレーム更新情報と、センサ１０が入力した画像とを用いて、センサ１０すなわちカメラの地図更新後の位置姿勢（以降、更新位置姿勢と呼ぶ）を算出する。更新位置姿勢はＳ１３で説明した方法で算出するが、地図情報のかわりに、更新情報を用いる点が異なる。更新位置姿勢を算出したら、Ｓ１２４０に移る。

Ｓ１２４０では、差分取得手段１３０が、Ｓ１２３０で算出した更新位置姿勢と、位置算出部１３が算出したセンサの位置姿勢との距離を算出する。ここでいう位置姿勢の距離とは、Ｓ１２３０で算出したセンサ１０の座標と、位置姿勢推定部１３が算出したセンサ１０の座標のユークリッド距離のことである。このようにして算出したユークリッド距離の逆数を差分として、Ｓ１３０に移る。

＜効果＞
実施形態３では、地図の更新要素を用いてセンサの位置姿勢を算出し、更新前後のセンサの位置姿勢の変化量が増大する程、増加する差分を算出する。このようにすることで、移動体の急激な速度変化や方向変化が少なくなる。

＜変形例＞
本実施形態においては、更新情報を用いて地図更新後のセンサの位置姿勢を算出していた。地図更新前後でのセンサの位置姿勢の変化量を推定することができる方法であれば、上記方法に限らない。例えば、センサ周囲の近傍キーフレーム情報を用いるのではなく、位置姿勢推定部１３がセンサ１０の位置姿勢を算出した際に用いた地図要素を用いてもよい。すなわち、位置姿勢推定部１３が算出した特徴点を識別しておく。更新情報に含まれる識別した特徴点と一致する特徴点情報の更新値に基づいて、差分取得部１２０が更新位置姿勢推定してもよい。

本実施形態においては、更新情報を用いてセンサの位置姿勢を算出していた。位置姿勢推定をせず、周囲の地図要素の変化量からセンサの位置姿勢の変化量を推定することもできる。具体的には、特徴点の更新前と更新後の座標の距離が最小となるよう剛体変換を求め、これをセンサの位置姿勢の変化量とみなす。また、キーフレームの更新前と更新後の座標の距離が最小となる剛体変換を求めてもよい。

差分の算出に、更新前後の算出するセンサの座標の距離の逆数を用いていたが、距離が増大するほど、差分の値が減少するような設定方法であれば何でもよい。逆余接関数としてもよいし、底を距離値の負値とした指数関数を用いてもよいし、シグモイド関数に距離値を代入し、−１をかけ、１を足した関数を用いてもよい。

＜実施形態４＞
実施形態３においては、地図更新前後での算出するセンサの位置姿勢の変化量に基づき差分を算出していた。実施形態４では、地図更新前後で算出する移動体の制御値の変化量に基づいて差分を算出する。具体的には、地図更新によって移動体の方位が大きく変わる場合には大きな影響があると算出する方法について述べる。

実施形態４における装置の構成、および処理ステップの図は、実施形態１で説明した情報処理装置１の構成を示す図１と同一であるため省略する。実施形態１と異なるのは、差分取得部１２０が差分を算出する処理ステップである。

実施形態４における処理全体の手順は、実施形態１で説明した情報処理装置１の処理手順を示す図２と同一であるため、説明を省略する。実施形態１と異なり、差分取得Ｓ１２０において、差分取得手段１２０が、地図更新後に位置姿勢推定部１３が算出するセンサの位置姿勢を推定するだけでなく、さらに、地図更新後に制御値決定部１７が算出する制御値を推定する。また、地図更新前後の制御値の変化量を算出する。

図６は、本実施形態４における差分取得手段１２０の処理の詳細を示した図である。Ｓ１２１０からＳ１２３０は実施形態３で説明した処理手順と同一である。実施形態３と異なるのは、Ｓ１２３０で更新後に算出するセンサの位置姿勢に基づいて、さらにＳ１３１０で、地図更新後の制御値、および地図更新前後の制御値の変化量を算出する点である。本実施形態においては、制御値とは作動二輪タイヤの左右のタイヤそれぞれの回転数のことであり、制御値の変化量とは、地図更新前後の各タイヤの回転数の差分のことである。

Ｓ１３１０では、地図更新前後の制御値の変化量を算出する。差分取得手段１３０がＳ１３３０で算出した、地図更新後のセンサの位置姿勢に基づき、まず制御値の算出を行う。制御値の算出には、地図更新後のセンサの座標と、移動体の目標位置とのユークリッド距離を小さくするＤＷＡ法で算出する。なお、本実施形態においては、不図示の入力手段により、差分取得手段に移動体の目標位置が入力されている。次に算出した地図更新後の制御値と、制御値決定部１７が算出した制御値をとの差分を算出する。本実施形態においては、制御値とは作動二輪タイヤの回転数である。作動二輪タイヤの地図更新前後の回転数の差の平均値を、差分として算出する。

＜効果＞
実施形態４では、地図更新前後で算出する移動体の制御値の変化量が増大する程、増大する差分を算出する。このようにすることで、移動体の急激な速度変化や方向変化が少なくなる。

＜変形例＞
実施形態４では、差分として、作動二輪タイヤの地図更新前後の回転数の平均値としていたが、地図更新前後の制御値の差を小さくするものであればなんでもよい。本実施形態のように作動二輪タイヤを用いているのであれば、地図更新前後の回転数の差が大きい方の値を差分としてもよい。小さい方の値を差分としてもよい。所定の重みをかけた重み付きの回転数の差を差分としてもよい。また、本実施形態においては、制御部が作動二輪タイヤの場合について説明したが、地図更新に伴う制御値の差が増大する程増大する差分として算出できるものであれば、制御部の形態によらない。

実施形態４では、制御値の算出にＤＷＡを用いていた。ＤＷＡに限らず、地図更新前後で比較できる制御値を算出できる方法であればどのような方法を用いてもよい。ある一時刻ではなく、将来の複数の時刻における制御値のバリエーションを算出し、複数時刻の位置姿勢（予測軌跡）を算出し、予測軌跡を辿るような制御値を選択する手法であるＧｒａｐｈ Ｓｅａｒｃｈ Ａｐｐｒｏａｃｈを使用してもよい。また、現在位置から二次元空間の中から障害物のない一定領域をランダムにサンプリングする。その地点をノードとして更に周囲の空間をランダムにサンプリングすることを繰り返して生成した予測軌跡を選択するＲａｎｄｏｍｉｚｅｄ Ａｐｐｒｏａｃｈなど、任意の手法を用いることができる。

また、実施形態４では直接制御値を算出して比較していたが、制御値を算出せずとも、制御値を算出するための要素の変化量を差分としてもよい。具体的には、相対目標位置の変化量のことである。ここでいう相対目標位置の変化量とは、地図更新前のセンサ１０の位置姿勢に対する目標位置の座標と、地図更新前のセンサ１０の位置姿勢に対する目標位置の座標の、ユークリッド距離である。算出したユークリッド距離を、差分値として用いる。ユークリッド距離に限らず、地図更新前後のセンサ１０からみた目標位置の方位差や、二つの目標位置の差分ベクトルのノルムなど、任意である。

＜実施形態５＞
実施形態１から４では、地図更新に伴うセンサの位置姿勢や移動体の制御値の変化等の、情報処理システムの内部状態を基に差分を算出していた。実施形態５では、移動体の周囲の状況に応じて差分を算出する。本実施形態においては、移動体の周囲の状況とは、周囲の物体の配置のことである。移動体が物体に近ければ地図更新を行わず、物体から離れた場合に地図更新を行うような構成を実現する方法について説明する。

実施形態５における装置の構成、および処理ステップの図は、実施形態１で説明した情報処理装置１の構成を示す図２と同一であるため省略する。実施形態１と異なるのは、差分取得手段１３０が差分を算出する処理ステップの処理内容である。ハードウェア構成は実施形態１と同様に図３のような構成を用いる。

実施形態５における処理全体の手順は、実施形態１で説明した情報処理装置１の処理手順を示す図４と同一であるため、説明を省略する。実施形態１と異なるのは、差分取得Ｓ１３０において、差分取得手段１３０が、移動体の周囲の状況として、特徴点とセンサとの距離値に基づいて差分を算出する点である。

Ｓ１３０において、差分取得部１２０が、地図保持部１４が保持する地図情報に含まれるすべてのキーフレーム情報に含まれる各特徴点の三次元位置と、位置姿勢推定部１３０が算出した位置とのユークリッド距離を算出する。算出した個々のユークリッド距離のうち、最小値の逆数を差分として算出する。

＜効果＞
実施形態５においては、周囲の状況すなわち周囲の物体との距離に応じて差分を算出する。つまり、周囲の物体との距離が近いほど、差分の値を大きくする。このようにすることで、移動体と周囲の物体との距離が近いほど、急激な速度変化や方向変化が少なくなる。

＜変形例１＞
本実施形態においては、周囲の状態とは周囲の物体の配置のことであり、差分とはセンサと特徴点の距離に基づいて算出していた。しかしながら、センサと周囲の物体との距離を測れる方法であればなんでもよい。

例えば、センサ１５が奥行値を取得できるデプスカメラやステレオカメラ等の構成である場合は、それらのカメラから取得できる奥行値を周囲の物体との距離として用いてもよい。

また、画像を入力してデプスマップ（画像の各画素に奥行値を格納したデータ構造）を推測するように学習したＣＮＮ（Ｃｏｎｖｏｌｕｔｉｏｎａｌ Ｎｅｕｒａｌ Ｎｅｔｗｏｒｋ）を用いて取得した奥行値を周囲の物体との距離として用いてもよい。

センサ１５がＬｉＤＡＲの場合は、周囲の計測点との距離を周囲の物体との距離として用いてよい。

赤外線センサや超音波センサ等の距離センサを移動体が保持する構成とすれば、それらの計測値を周囲の物体との距離として用いてもよい。

移動体に距離センサを搭載せずとも、監視カメラや近接センサなど移動体以外の物体に搭載したセンサを用いて移動体と周囲の物体との距離を計測してもよい。不図示の保持手段に移動体の周囲のレイアウト図を保持していれば、位置姿勢推定部１３が算出したセンサの位置姿勢とレイアウト図から周囲の物体との距離を算出してもよい。

本実施形態においては、センサと周囲の物体との距離を差分取得に用いていた。移動体はある空間中に体積を持っているため、移動体の形状モデルを保持しておき、移動体の表面から物体への最短距離を本実施形態の距離値として用いてもよい。このようにすることで、より正確に周囲の物体と移動体との距離を正確に測ることができ、より正確に差分を算出することができるため、より安全に、安定して移動体を運用することができる。

差分の算出に、センサと周囲の各特徴点との距離の最小距離の逆数を用いていたが、距離が増大するほど、差分の値が減少するような設定方法であれば何でもよい。逆余接関数としてもよいし、底を距離値の負値とした指数関数を用いてもよいし、シグモイド関数に距離値を代入し、−１をかけ、１を足した関数を用いてもよい。また、差分の算出に、センサと周囲の各特徴点との距離の最小距離を用いていたが、センサから所定の距離内にある特徴点との距離の平均値や中央値でもよい。

＜変形例２＞
本実施形態においては、物体との距離に応じて差分を算出していたが、周囲の状況として、物体の属性を用いてもよい。センサ１５としてカメラを用いている場合は、カメラの映像から事前に物体の種別を判定するように学習したＣＮＮを用いて物体を検出し、物体の種別や個数に応じて、差分を算出することもできる。例えば、人や他の移動体が付近にいれば差分を増大させてもよい。差分を増大させることによって、地図更新に伴って移動体が急激に向きや速度を変えるリスクを抑制する。人や他の移動体が多いほど差分を増大させてもよい。さらには、ドアや階段の近くでは差分を増大させてもよい。壊れやすい物体を認識して差分を増大させてもよい。周囲の物体の重量が増大する程差分を増大させてもよいし、アプリケーションによっては逆に重量が低下する程増大する差分を算出してもよい。また、周囲の物体の移動速度が大きいほど差分を増大させてもよい。なお、周囲の物体の移動速度がわかる方法であれば、計測方法は任意である。時系列画像から周囲の移動物体の位置の変化を計測してもよいし、周囲の移動物体が他の移動体であれば不図示の移動体管理システムから他の移動体の移動速度を受信してもよい。

このようにすることで、人や他の移動体、ドアや階段、高価な物体などが付近に存在するとき、周囲の物体の移動物体の速度が大きい時に、地図更新に伴う移動体の急激な速度や方位の変化が起きにくくなり、より安全に、安定して移動体を運用できる。

＜変形例３＞
周囲の状況として、移動体の速度や方位が急激に変化させたくない地点で差分が増大するように、地点情報を用いて差分を算出する構成としてもよい。具体的には、例えば、曲がり角や交差点では差分を増大させてよい。また、地図や運行ルートに例えばドアや自動機といった特定物体が登録されていれば差分を増大させてもよい。また、狭いルートであれば差分を増大させるよう構成してもよい。さらに言えば、運行ルートに基づき、他の移動体が付近を運行しうるルートを走行中であれば差分を増大させてもよい。

そのような地点情報は、地図保持手段が保持する地図やルート情報から当該領域を検出すればよいし、通信Ｉ／Ｆ（Ｈ１７）を介して不図示の移動体管理システムから入力する構成としてもよい。

また、事前に人が不図示の入力手段によって設定した、侵入検知領域や、地図更新禁止区域といった特定の設定領域に、センサ１０が位置していれば差分を増大させる構成としてもよい。

＜実施形態６＞
実施形態５では、移動体の周囲の物体情報すなわち物体との距離に基づいて差分を算出していた。実施形態６では、移動体の状況に基づいて差分を算出する。本実施形態における移動体の状況とは、移動体の質量のことである。移動体の質量が増大する程、移動体の急激な速度や方位変化に伴う運動エネルギーが増す。このような場合に、移動体の質量が大きいほど、大きな差分を算出する構成について説明する。

実施形態６における装置の構成、および処理ステップの図は、実施形態１で説明した情報処理装置１の構成を示す図１と同一であるため省略する。実施形態と異なるのは、差分取得手段１２０が差分を算出する処理ステップである。ハードウェア構成は実施形態１と同様に図３のような構成を用いる。

実施形態６における処理全体の手順は、実施形態１で説明した情報処理装置１の処理手順を示す図２と同一であるため、説明を省略する。実施形態１とは、差分取得Ｓ１２０において、差分取得手段１２０が、移動体の状態に基づいて差分を算出する点である。

Ｓ１２０において、差分取得手段１２０が、不図示の保持手段に保持した移動体の質量情報を取得する。本実施形態においては、移動体の質量を差分値として用いる。

＜効果＞
実施形態６においては、移動体の状態、すなわち移動体の質量が増大する程、差分を増大させる。このようにすることで、より質量が大きな移動体程、地図更新に伴う移動体の急激な速度変化や方向変化が少なくなる。

＜変形例１＞
本実施形態においては、移動体の状況として、移動体の質量に基づいて差分を算出していた。しかしながら、移動体の運動エネルギーや位置エネルギーが増大する程大きな差分を算出する方法であればよい。

移動体が物体を運搬する場合には、運搬物を含めて移動体の質量としてよい。なお、運搬物の質量の入力方法は問わず、通信Ｉ／Ｆ（Ｈ１７）を介して不図示の移動体管理システムから取得してもよいし、不図示のＧＵＩから利用者が入力してもよいし、移動体に搭載した不図示の重量計量装置の計測値をＩ／Ｏから入力する構成にしてもよい。

すなわち、移動体の移動速度や角速度が大きいほど大きな差分を算出することもできる。

また、移動体がドローンといった空中を浮遊する場合には、高度が増大する程大きな差分を算出することもできる。

移動体の状況は、移動体のタスクの優先度や重要度に従って決める構成としてよい。例えば、移動体の状況として、移動中、停止中、荷物運搬中、充電中、荷物積み込み中といった移動体の運行情報を用いてもよい。運行情報に基づく差分値はあらかじめ利用者が不図示の入力手段を用いて入力しておく。こうすることで、例えば、荷物を運搬中には地図更新をしない、停止中や充電中など、位置計測を利用していない場合に地図更新をするように、移動体の運行状態に応じて地図を更新することができるようになる。

移動体の状況として、移動体の属性に基づいて差分を算出してもよい。例えば、移動体の剛性が高いほど、高い差分を算出してもよい。また、移動体の制動距離が長い移動体程、高い差分を算出するように構成してもよい。障害物センサの検知距離設定パラメータが小さいほど、高い差分を算出してもよい。移動体自体の価格を移動体の属性として用いてもよい。

また、移動体の状況として、搭載している運搬物の属性を基に差分を算出してもよい。例えば、運搬物の価格を差分取得手段１２０が、不図示の移動体管理システムから取得し、価格が増大する程、高い差分を算出することもできる。また、運搬物の剛性度合が高い程、重量が増大する程、高い差分を算出するようにしてもよい。また、運搬物の数が増大する程高い差分を算出してもよいし、運搬物が安定して積み上げられていなければ高い差分を算出するように構成してもよい。安定して積み上げられるとは例えば荷物の傾きが小さいということであり、不図示のカメラで荷物を撮影し、荷物の傾きを計測して判別する。このように、運搬物の属性に応じて、地図更新に伴う移動体の急激な速度や方位変化を少なくすることができる。

＜変形例２＞
さらに、実施形態１から６で説明した差分を組み合わせ、差分取得手段１２が合成差分を算出してもよい。合成差分は、地図更新前後に算出するセンサの位置姿勢や移動体すなわち移動体の制御値の変化が増大する程、合成差分が増大するような方式であれば、組み合わせ方は任意である。また、実施形態５や６で述べたように、移動体の状態や周囲の物体の属性に基づき、地図更新に伴う移動体の急激な速度や方位変化を少なくすることができる方法であればよい。すなわち、実施形態１から６で説明した方法で算出した各種差分の重み付き和でもよいし、重み付き積であってもよい。さらには、特定の実施形態の方法のみ組み合わせて合成差分を算出してもよい。また、更新方法決定手段１４が、合成差分を用いて更新方法を決定してもよい。このようにすることで、実施形態１から６で説明した各種条件を満たすよう、地図更新を行うことができるようになり、より安全に、安定して移動体すなわち移動体を運行することができる。

＜実施形態７＞
実施形態１から６では、本発明を移動体に適用した事例について説明した。本発明は移動体に搭載するに限らず、センサと地図情報を基に位置姿勢推定し、地図を更新しうるすべての構成に対して適用できる。本実施形態は、その応用事例の一つとして、本発明の処理装置をＭＲ（Ｍｉｘｅｄ Ｒｅａｌｉｔｙ）やＡＲ（Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ）、ＶＲ（Ｖｉｒｔｕａｌ Ｒｅａｌｉｔｙ）を実現するデバイスに搭載する。デバイスは、例えば、ＨＭＤ（Ｈｅａｄ ｍｏｕｎｔｅｄ Ｄｉｓｐｌａｙ）やスマートフォン、タブレットといったデバイスである。それらのデバイスに搭載し、ＣＧの提示位置の算出に用いる情報処理装置に用いる構成について説明する。

図７は、本実施形態における情報処理装置１をＭＲシステムに適用した場合のハードウェア構成例を示す図である。センサ１０、表示装置２０、センサ情報入力部１２、位置姿勢推定部１３、地図保持部１４、更新情報生成部１５、地図更新部１６、表示情報生成部２１から構成される。さらに情報処理装置１は更新情報入力部１１０、差分取得部１２０、更新方法決定部１３０からなる。

センサ１０、センサ情報入力部１２、位置姿勢推定部１３、地図保持部１４、更新情報生成部１５、地図更新部１６、更新情報入力部１１０、差分取得部１２０、更新方法決定部１３０は、実施形態１で説明した構成と同等であるため説明を省略する。実施形態１と異なるのは表示装置２０、画像構成部２１が追加されている点である。

表示情報生成部２１は、位置姿勢推定部１３が入力した位置姿勢と不図示の保持部が保持するカメラの内部・外部パラメータを用いて仮想物体のＣＧ画像をレンダリングし、センサ情報入力部１２が入力した入力画像上にＣＧを重畳した合成画像を生成する。また、合成画像を表示部１３に出力する。

表示装置２０は、モバイル端末のディスプレイであり、表示情報生成部２１が生成した合成画像を表示する。

実施形態７における処理手順は、実施形態１で説明した情報処理装置１の処理手順を示す図２と同一であるため、説明を省略する。実施形態１とは、制御値決定Ｓ１４の代わりに、表示情報生成ステップを行う。

表示情報生成ステップでは、表示情報生成部２１が、位置姿勢推定部１３がＳ１３で導出したセンサ１０の位置姿勢を用いて、仮想物体のＣＧ像をレンダリングし、入力画像に重畳合成した合成画像を生成して、表示装置２０に入力し、提示する。

＜効果＞
実施形態７では、地図更新前後で算出する位置姿勢の変化量が小さくなるように地図更新を行うことで、利用者がＭＲ，ＡＲ，ＶＲを利用している空間近辺での地図更新による位置姿勢の急激な変化が小さくなる。これにより、ＣＧずれやＣＧ飛びを小さく、利用者が気づきにくく提示することができ、利用者のＭＲ，ＡＲ，ＶＲ体験を向上させることができる。

本実施形態においては実施形態１で説明した方法で差分を算出する方右方を説明したが、本実施形態における差分は、地図更新による位置姿勢の急激な変化が小さくなる方法であれば実施形態１から６で説明した任意の方法で算出することができる。実施形態２のように更新要素の変化量を用いてもよい。実施形態３のように地図更新前後で算出するセンサの位置姿勢の変化量に基づいて算出してもよい。本実施形態においては実施形態４のような制御値は算出しないが、目標位置をＣＧの提示位置と読み替え、ＣＧの位置が変化しないように差分を算出することができる。また、実施形態５のように周囲の物体との距離に応じて差分を算出してもよい。実施形態６のように、ＭＲシステムの状態、すなわちＭＲシステムの移動速度や、運行状態としてＣＧ提示中か否か、ＭＲシステムの重量に基づいて差分を算出してもよい。

さらには、ＣＧコンテンツの種別や大きさといった属性を基に差分を算出してもよい。すなわち、使用者がよりＣＧのブレに気が付きやすいような属性を差分として算出する。例えば、ＣＧの大きさが増大する程、高い差分を算出してもよい。コントラストが高い程、高い差分を算出してもよい。ＣＧが視野の中心にある程、高い差分を算出してもよい。さらに言えば、不図示のアイトラッキング装置を搭載する構成として、眼球運動が小さいほど、差分の値を増大させてもよい。このような構成とすることで、地図更新に伴う利用者の知覚するＣＧブレが減少し、より使用者が快適にＭＲ，ＡＲ，ＶＲを体験できる。

＜実施形態８＞
実施形態１から７では、地図更新に伴う、算出するセンサの位置姿勢や移動体の制御値への差分に基づいて、地図更新方法を決定していた。実施形態７では、地図更新に伴って移動体の速度や方位が急激に変化しないよう、あらかじめ移動体を制御する方法について説明する。ハードウェア構成は実施形態１と同様に図３のような構成を用いる。ここで、図３のようなハードウェアを移動体が搭載してもよい。移動体は周囲をセンサで計測した結果を用いて自身の制御や地図更新を行う。また、移動体自体にハードウェアを搭載せず、工場全体の制御を行う上位システムや移動体の制御を行う中位システムにおいて図３のようなハードウェア構成例を実施してもよい。その場合は、通信Ｉ／Ｆによってそれぞれの移動体とシステムとがセンサ情報や地図情報をやり取りしながら地図情報を更新する。

図８は、本実施形態における情報処理装置２０の機能構成例を示す図である。本実施形態における情報処理システムは、センサ１０、制御装置１１、センサ情報入力部１２、位置姿勢推定部１３、地図保持部１４、更新情報入力手段１５、地図更新部１６を有する。および、情報処理装置２である、更新情報入力部１１０、更新位置姿勢推定部２１０、制御値決定部２２０から構成されている。このうち、センサ１０、制御装置１１、センサ情報入力部１２、位置姿勢推定部１３、地図保持部１４、更新情報入力手段１５、地図更新部１６、更新情報入力部１１０については、実施形態１と同一であるため説明を省略する。

実施形態１と異なるのは、差分取得手段１２が取り除かれ、更新位置姿勢推定部２１０、制御値決定部２２０が新たに情報処理装置２に追加された点である。

更新位置姿勢推定部２１０は、更新情報入力部１１０が入力した更新情報に基づいて、位置姿勢推定部１３が算出する地図更新後のセンサの位置姿勢を予測する。すなわち、更新位置姿勢推定部２１０は、取得された新たな特徴点に基づいて推定される移動体の予測位置姿勢（第２の位置姿勢）、を推定する。算出した予測位置姿勢を、制御値決定部２２０に出力する。

制御値決定部２２０は、位置姿勢推定部１３が算出した位置姿勢を、更新位置姿勢推定部２１０が算出した予測位置姿勢に基づき、それらの位置の差が小さくなるように制御値を算出する。算出した制御値を、制御装置１１に出力する。

図９は、本実施形態における情報処理装置２の処理手順を示すフローチャートである。処理ステップは、初期化Ｓ１１、センサ情報入力Ｓ１２、位置姿勢推定Ｓ１３、地図更新判定Ｓ１５、更新情報生成Ｓ１５，地図更新Ｓ１６、終了判定Ｓ１７、更新情報入力Ｓ１１０、更新位置姿勢推定Ｓ２１０、制御値決定Ｓ２２０から構成されている。

初期化Ｓ１１、センサ情報入力Ｓ１２、位置姿勢推定Ｓ１３、地図更新判定Ｓ１５、更新情報生成Ｓ１５，地図更新Ｓ１６、終了判定Ｓ１７、更新情報入力Ｓ１１０については実施形態１と同一であるため説明は省略する。

更新位置姿勢推定Ｓ２１０では、更新位置姿勢推定部２１０が、更新情報入力部１１０が入力した更新情報に基づき、地図更新後に位置姿勢推定部１３が算出する位置姿勢を予測位置姿勢として推定する。具体的には、更新情報に含まれるキーフレーム更新情報中の各キーフレーム更新情報が保持する特徴点情報の中から、センサから所定の距離以内にある特徴点を選択し、地図更新前後の当該特徴点の移動量から剛体変換を求める。剛体変換とは、ある三次元座標を別の三次元座標に一意に変換する変換行列のことであり、本実施形態では４行４列の行列である。このような構成における剛体変換は、特徴点の更新前と更新後の位置との距離が最小となるよう求める。これをセンサの位置姿勢の変化量として算出し、地図更新前のセンサ位置姿勢に掛け合わせることで予測位置姿勢を求める。

制御値決定Ｓ２２０では、更新位置姿勢推定部２１０が推定した予測位置姿勢となるように、移動体を制御する制御値を算出する。具体的には、予測位置姿勢を目標として、位置姿勢推定部１３が算出したセンサ１０の位置姿勢とのユークリッド距離を小さくするような制御値を算出する。本実施形態では、制御値の算出にはＤＷＡ法を用いる。

＜効果＞
実施形態８では、地図の更新に伴う要素の変化に基づき、地図更新後に算出するセンサの位置姿勢をあらかじめ予測し、予測位置姿勢になるように移動体をあらかじめ移動させる制御値を算出する。このようにすることで、移動体の急激な速度変化や方向変化が少なくなる。

＜変形例＞
実施形態８では、入力した位置姿勢の周囲の地図の要素の更新量に基づいて、地図更新後のセンサの位置姿勢を予測していた。地図更新後のセンサの位置姿勢を算出する方法として、地図要素の更新値とセンサ情報を用いて、位置姿勢を算出してもよい。具体的には、更新位置姿勢推定部２１０が、画像と更新情報に含まれるキーフレーム群更新情報を用いて、位置姿勢推定部１３が位置姿勢を算出するのと同様の方法で、地図更新後のセンサの位置姿勢を算出することもできる。

実施形態８では、更新位置姿勢推定部２１０が推定した予測位置姿勢となるように、制御値を算出した。算出する制御値は、移動体の速度や方位が急激に変化することを低減することができればこの方法に限らない。例えば、移動体の急激な移動方向の変化を防ぐように、あらかじめ姿勢のみ、予測姿勢と一致するような制御値を算出してもよい。移動体の進行方向へ位置姿勢を予測した場合には減速、逆に進行方向逆への位置姿勢変化が予測される場合には加速するような制御値を算出しておいてもよい。

実施形態８では、地図更新後のセンサの予測位置姿勢になるような制御値を算出した。一方、センサの予測位置に替えて、地図更新に伴う制御値の予測値を算出し、地図更新前の制御値と予測制御値との差が小さくなるような制御値を算出しておいてもよい。具体的には、地図更新後のセンサの予測位置姿勢に基づいて算出した制御値と、現在の制御値の重み付き平均により、算出する。このようにすることで、急激な制御値の変化を減らすことができる。

実施形態８では、地図更新後の位置姿勢となるような制御値を算出した。さらに、地図更新部１６が移動体から制御の進み具合を受信する構成として、地図更新後の位置姿勢となるまで地図更新の実行を待つ構成にしてもよい。このように、移動体の制御が完了し、地図更新による位置姿勢の変化の影響が小さくなってから地図を更新することで、より安全に、安定して移動体を運用することができる。

＜実施形態９＞
実施形態８では、移動体が地図更新後の位置姿勢となるような制御値を算出した。また、実施形態１から６で説明した差分に基づいて制御値を算出することもできる。本実施形態においては、実施形態１で説明した差分を用いて、地図更新に伴う差分が所定より大きな範囲から移動体が離脱するような制御値を算出する構成を説明する。

本実施形態における情報処理装置の構成は、実施形態８と同一であるため説明を省略する。ハードウェア構成は実施形態１と同様に図３のような構成を用いる。実施形態８と異なるのは、実施形態８に示した更新位置姿勢推定部２１０が実施形態１で説明した差分取得部１２０にかわり、差分取得部１２０が算出した差分を制御値決定部２２０が入力する。差分が小さくなる地点へ移動するような制御値を算出する点である。また、差分取得部が、不図示の保持手段が保持する移動経路情報を受信する。移動経路情報とは、目的地に向かうまでの座標のリストのことである。

本実施形態における処理ステップは、実施形態８で説明した処理ステップの図８と同一であるため説明を省略する。実施形態８と異なるのは、更新位置姿勢推定ステップ２１０が、実施形態１で説明した差分取得Ｓ１２０にかわっている点、および制御値決定Ｓ２２０の処理内容である。

本実施形態における制御値決定Ｓ２２０では、制御値決定部２２０が、差分取得Ｓ１２０において差分取得手段１２０が算出した差分が所定の値より大きな地図領域外を目標位置として移動体の制御値を算出する。地図領域外の目標位置は、現在のセンサの位置姿勢から移動体の移動経路情報を順に辿り、初めて地図領域外となった点とする。当該目標位置へ移動するための制御値を、ＤＷＡ法で算出する。

＜効果＞
実施形態９では、差分に基づいて算出した地図更新に伴うセンサの位置姿勢や移動体の制御値の変化が大きい領域外に移動体が移動するように制御する。このようにすることで、移動体の急激な速度変化や方向変化が少なくなる。

＜変形例＞
実施形態９における、差分取得Ｓ１３０は、実施形態１から６で述べた差分の算出方法のどの方法を用いてもよい。

制御値決定部は、差分が低下するような制御値を算出すればどのような方法を用いてもよい。具体的には、例えば更新範囲から距離が離れるような制御値や、地図の構成要素の変化量が少ない地点に移動するような制御値、移動体と周囲の物体との距離が増大する地点へ移動する制御値である。

差分が低下する地点を目標位置の探索方法も任意である。移動体の地図領域を格子状に区切り、各格子で差分を算出し、差分が最も低い格子を目標位置として算出してもよい。こうすることで、あまり遠くへの移動をせずに地図更新を行う。また、移動体の進行予定の移動ルート上をランダムに選択し、所定より低い差分となる地点が選ばれるまで目標地点を選びなおしてもよい。こうすることで、移動体のルートを変えずに、所定の目標地点に到達したときに地図更新する。

実施形態９では、差分が所定より小さな地点へ移動体を移動する制御値を算出していた。しかしながら、地図更新に伴って移動体の速度や方位が急激に変化しない制御値を算出する方法であれば、上記方法に限らない。例えば、差分が所定の値より大きな地点に移動体が位置している場合には、移動体の制御値の変化量の最大値を所定以下になるように制御値を算出してもよいし、あらかじめ移動体の速度や角速度を所定の値以下になるような制御値を算出してもよい。一時停止するような制御値を算出してもよい。また、周囲の物体との距離が増大するように制御値を算出してもよい。

実施形態９では、差分が所定より小さな地点へ移動体を移動させる制御値を算出していた。一方、位置姿勢推定に用いる地図要素が所定の変化量より小さくなる地点を差分から求め、その地点へ移動するように制御値を算出してもよい。

そのような地点の算出方法も任意である。例えば、地図上の任意の地点におけるセンサで計測できる範囲を算出し、その範囲にある地図更新に伴う移動量が所定の値より大きな特徴点割合が所定より少ない地点を算出してよい。地図更新に伴う移動量が所定の値より大きなキーフレームの割合が所定より少ない地点を算出してよい。

＜効果のまとめ＞
実施形態１では、更新領域からセンサ、すなわち移動体が離れたときに地図更新を許容することで、更新における位置計測値の変化量を小さくすることができる。これにより、移動体の急激な速度変化や方向変化が少なくなる。

実施形態２では、更新要素の変化量に基づいて、地図の更新方法を算出する。すなわち、地図更新要素の変化が所定値以下であれば更新し、更新方法が所定値以上であれば更新しない。このような構成にすることで、更新における位置計測値の変化量が所定以上にならないようにする。このようにすることで、移動体の急激な速度変化や方向変化が少なくなる。

実施形態３では、地図の更新要素を用いてセンサの位置姿勢を算出し、更新前後のセンサの位置姿勢の変化量が増大する程、増加する差分を算出する。このようにすることで、移動体の急激な速度変化や方向変化が少なくなる。

実施形態４では、地図更新前後で算出する移動体の制御値の変化量が増大する程、増大する差分を算出する。このようにすることで、移動体の急激な速度変化や方向変化が少なくなる。

実施形態５においては、周囲の状況すなわち周囲の物体との距離に応じて差分を算出する。つまり、周囲の物体との距離が近いほど、差分の値を大きくする。このようにすることで、移動体と周囲の物体との距離が近いほど、急激な速度変化や方向変化が少なくなる。

実施形態６においては、移動体の状態、すなわち移動体の質量が増大する程、差分を増大させる。このようにすることで、より質量が大きな移動体程、地図更新に伴う移動体の急激な速度変化や方向変化が少なくなる。

実施形態７では、地図更新前後で算出する位置姿勢の変化量が小さくなるように地図更新を行うことで、利用者がＭＲ，ＡＲ，ＶＲを利用している空間近辺での地図更新による位置姿勢の急激な変化が小さくなる。これにより、ＣＧずれやＣＧ飛びを小さく、利用者が気づきにくく提示することができ、利用者のＭＲ，ＡＲ，ＶＲ体験を向上させることができる。

実施形態８では、地図の更新に伴う要素の変化に基づき、地図更新後に算出するセンサの位置姿勢をあらかじめ予測し、予測位置姿勢になるように移動体をあらかじめ移動させる制御値を算出する。このようにすることで、移動体の急激な速度変化や方向変化が少なくなる。

実施形態９では、差分に基づいて算出した地図更新に伴うセンサの位置姿勢や移動体の制御値の変化が大きい領域外に移動体が移動するように制御する。このようにすることで、移動体の急激な速度変化や方向変化が少なくなる。

＜定義のまとめ＞
更新情報入力部は、地図要素の更新情報を入力するものであれば何でもよい。例えば、地図要素がキーフレームや特徴点であれば、キーフレームの位置姿勢や特徴点の三次元座標の更新値を入力してよい。なお、キーフレームとは、センサの位置姿勢を算出するための最小単位の地図要素のことである。特徴点とは、位置姿勢推定のために必要な指標のことである。地図要素がＯｃｃｕｐａｎｃｙ Ｇｒｉｄ Ｍａｐ（通行可能領域と通行付加領域すなわち壁等の領域とを格子にプロットしたデータ構造である地図）であれば、各格子に保持する値の更新値を入力してもよい。

差分取得部は、地図更新に伴うセンサの位置姿勢の変化量が増大する程大きくなるような値を算出するものであれば何でもよい。

例えば、地図の更新領域に基づいて算出してよい。すなわち、地図の更新領域の広くなる程、更新領域とセンサとの距離が近くなる程大きな値を算出してよい。更新領域にセンサが位置していれば、していないときより大きな差分値となるように算出してもよい。

地図の更新要素の変化量に基づいて算出してもよい。すなわち、地図更新に伴う地図の要素の移動量、センサの位置姿勢を算出するために用いる地図要素のうち地図更新に伴う地図の要素の変化量が所定より大きな地図要素の割合でもよい。

地図更新前後に算出するセンサの位置姿勢の変化量が大きくなるほど差分値を増大させるようにする。

移動体を制御するための制御値の地図更新に伴う変化量が大きいほど、大きな差分を算出するようにしてもよい。

前記センサの周囲の状況に基づいて差分を算出してもよい。すなわち、周囲の物体との距離が近いほど大きな差分を算出してもよい。特定の周囲の状況として例えば周囲物体の種別、大きさ、数、移動速度、重量、価格が増大する程、差分が増大するように算出してもよい。

前記センサを搭載する移動体の状況に基づいて差分を算出してもよい。すなわち、移動体やそれが搭載する物体の重量が大きいほど高い差分を算出してよい。搭載した物体の積み上げ方の安定性が低いほど、高い差分を算出してもよい。

更新方法決定部は、差分が大きくなる程、地図更新をしないような値を算出するものであれば何でもよい。例えば、更新方法の値として０〜１に正規化した実数とし、差分の値が小さいほど更新方法が０に、差分の値が大きいほど更新方法が１近づくように構成してよい。この時、差分の値の正規化方法は任意である。すなわち、差分値の逆余接関数としてもよいし、差分値の負値を底とした指数関数を用いてもよい。シグモイド関数に差分値を代入し、−１をかけ、１を足した関数を用いてもよい。

制御値決定部は、地図更新に伴うセンサの位置姿勢の変化または地図更新に伴う制御値の変化が減少するような方法であれば、算出方法は何でもよい。例えば、地図更新後の位置姿勢となるように移動体を制御する制御値を算出してもよいし、地図更新後に算出する制御値に近づくように制御値を算出してもよい。また、差分取得部が算出した差分が減少するように、制御値を算出してもよい。

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

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

１ 情報処理装置
１０ センサ
１１ 制御装置
１２ センサ情報入力部
１３ 位置姿勢推定部
１４ 地図保持部
１５ 更新情報生成部
１６ 地図更新部
１７ 制御値決定部
１１０ 更新情報入力部
１２０ 差分取得部
１３０ 更新方法決定部

Claims (15)

1. 移動体が走行する環境で予め計測された特徴点の位置を含む地図情報と、前記環境を前記移動体が移動中に計測した特徴点を含むセンサ情報とに基づいて、前記地図情報に含まれる特徴点の新たな位置を取得する取得手段と、
   前記地図情報に含まれる特徴点の位置から推定される前記移動体の第１の位置と、前記取得された新たな特徴点の位置に基づいて推定される前記移動体の第２の位置と、の変化の大きさが閾値より大きい場合は前記地図情報の更新を抑制する更新手段と、
   を有することを特徴とする情報処理装置。
2. 前記更新手段は、前記地図情報に含まれる特徴点の位置と、当該特徴点の特徴が類似する特徴点で前記センサ情報に含まれる特徴点の位置との差が閾値より小さい場合、前記取得された新たな特徴点の位置に基づいて前記地図情報を更新し、前記地図情報に含まれる特徴点の位置と、前記センサ情報に含まれる前記特徴点の位置との変化の大きさが閾値より大きい場合は、前記地図情報の更新を抑制することを特徴とする請求項１に記載の情報処理装置。
3. 前記地図情報は、前記移動体の目標地点の位置についての情報を有し、
   前記更新手段は、前記第１の位置と前記移動体の目標地点の位置とに基づいて決定された第１の制御値と、前記第２の位置と前記移動体の目標地点の位置とに基づいて決定された第２の制御値と、の変化の大きさが閾値より大きい場合は前記地図情報の更新を抑制することを特徴とする請求項１または２に記載の情報処理装置。
4. 前記更新手段は、前記第１の位置と前記第２の位置との変化の大きさが閾値より大きい場合は、複数回に分けて前記地図情報を更新することを特徴とする請求項１または２に記載の情報処理装置。
5. 前記取得手段は、前記地図情報に含まれる特徴点であって、前記センサ情報によって新たな位置が検出された特徴点が含まれる更新領域を更に取得し、
   前記更新手段は、前記移動体の第１の位置が前記更新領域に含まれない場合は、前記取得された新たな特徴点の位置に基づいて前記地図情報を更新することを特徴とする請求項１に記載の情報処理装置。
6. 前記更新手段は、前記移動体の第１の位置が前記更新領域に含まれる場合は、前記移動体が前記更新領域から所定の距離だけ離れたときに前記地図情報を更新することを特徴とする請求項５に記載の情報処理装置。
7. 前記取得手段は、前記センサ情報に基づいて、前記移動体の周囲にある他の物体と前記移動体との距離を示す情報を更に取得し、
   前記更新手段は、前記取得された情報に基づいて、前記移動体から所定の範囲内に他の物体がない場合は、前記地図情報を更新し、前記移動体から所定の範囲内に他の物体がある場合は、前記地図情報の更新を抑制することを特徴とする請求項１乃至６のいずれか１項に記載の情報処理装置。
8. 前記取得手段は、前記移動体に積まれた荷物の重さまたは前記移動体の重さについての情報を更に取得し、
   前記更新手段は、前記移動体または前記荷物の重さが閾値より大きい場合は前記地図情報の更新を抑制することを特徴とする請求項１乃至７のいずれか１項に記載の情報処理装置。
9. 前記センサ情報は、計測装置によって前記移動体と周囲の環境との距離を計測した結果であることを特徴とする請求項１乃至８のいずれか１項に記載の情報処理装置。
10. 前記地図情報は、前記環境における前記特徴点の位置を３次元座標で表した情報であって、
    前記地図情報における前記特徴点の３次元の位置と前記センサ情報から取得された前記特徴点とに基づいて前記移動体の第１の位置姿勢を推定し、前記センサ情報から取得された前記特徴点の３次元の位置に基づいて第２の位置姿勢を推定する推定手段を更に有することを特徴とする請求項１乃至９のいずれか１項に記載の情報処理装置。
11. 前記更新手段によって前記地図情報が更新されていることを示す情報を提示する提示手段を更に有することを特徴とする請求項１乃至１０のいずれか１項に記載の情報処理装置。
12. 前記センサ情報は、前記センサと周囲の物体との距離、周囲の物体の種別、大きさ、数、移動速度、重量、価格、の少なくともどれか一つを示す情報であることを特徴とする請求項１乃至１１のいずれか１項に記載の情報処理装置。
13. 前記地図情報を保持する地図保持手段を更に有し、
    前記更新手段は、更新された地図情報を前記地図保持手段に保持させることを特徴とする請求項１乃至１２のいずれか１項に記載の情報処理装置。
14. コンピュータを、請求項１乃至１３のいずれか１項に記載の情報処理装置が有する各手段として機能させるためのプログラム。
15. 移動体が走行する環境で予め計測された特徴点の位置を含む地図情報と、前記環境を前記移動体が移動中に計測した特徴点を含むセンサ情報とに基づいて、前記地図情報に含まれる特徴点の新たな位置を取得する取得工程と、
    前記地図情報に含まれる特徴点の位置から推定される前記移動体の第１の位置と、前記取得された新たな特徴点の位置に基づいて推定される前記移動体の第２の位置と、の変化の大きさが閾値より大きい場合は前記地図情報の更新を抑制する更新工程と、
    を有することを特徴とする情報処理方法。

Images