JP2020144710A - 自己位置推定装置、自己位置推定方法及びプログラム - Google Patents

Abstract

【課題】カメラを用いた自己位置推定処理の安定化を図ることができる自己位置推定装置等を提供する。【解決手段】移動体１に搭載されたカメラ１１から視界画像を取得する取得部１０００と、前記視界画像上における特徴点の位置と、世界座標上における複数の特徴点の位置が記録されてなる地図とに基づいて、前記世界座標上における前記移動体の位置及び姿勢を推定する自己位置推定処理を行う推定部１００１と、前記視界画像を、当該視界画像上に含まれる新たな特徴点を前記地図に追加するためのキーフレームとして登録する登録部１００２と、を備える。前記登録部は、前記移動体の移動が、当該移動体の位置及び姿勢の少なくともいずれかについて予め規定された条件を満たした場合に、前記視界画像を前記キーフレームとして登録する。【選択図】図２

Description

本開示は、自己位置推定装置、自己位置推定方法及びプログラムに関する。

移動体の位置及び姿勢を高精度に検出し、当該位置及び姿勢の検出結果を利用して移動体の操舵制御を行う技術が知られている。

特許文献１には、互いに異なるセンサからの入力に基づいて自己位置を推定する技術が開示されている。

移動体の位置及び姿勢を高精度に検出するための技術としてＬｉＤＡＲ（Laser Detection and Ranging）が知られているが、高価なセンサ（レーザスキャナ）を搭載することが求められる。その一方で、比較的安価なセンサであるカメラを用いた自己位置推定技術についても開発が進められている。

非特許文献１には、カメラを用いた自己位置推定技術であるＯＲＢ−ＳＬＡＭ（ORB - Simultaneous Localization and Mapping）をベースに、地図作成時にのみＬｉＤＡＲを援用し、実運用時にはカメラのみで高精度な自己位置推定を行う手法について記載されている。

特開２０１２−２４８０３２号公報

Adi Sujiwo, Eijiro Takeuchi, Luis Yoichi Morales, Naoki Akai, Yoshiki Ninomiya and Masato Edahiro, "Localization Based on Multiple Visual-Metric Maps" in Proc of IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems, 2017.

ＯＲＢ−ＳＬＡＭによる地図作成処理では、複数の特徴点の世界座標上の位置、及び、それらの特徴点を観測した視界画像（フレーム）である複数のキーフレームが記録されてなる地図が作成される。地図には、ＯＲＢ−ＳＬＡＭによる自己位置推定処理によって推定された、各キーフレーム取得時のカメラの位置及び姿勢も記録される。
ＯＲＢ−ＳＬＡＭによる自己位置推定処理では、カメラから取得された視界画像（フレーム）によって観測された特徴点と、地図上に記録された特徴点とを整合（マッチング）することで、その視界画像取得時のカメラの世界座標上における位置及び姿勢を推定する。この際、カメラから取得された視界画像に対し、特徴点の位置関係が最も近いキーフレーム（カレントキーフレーム）を特定し、このカレントキーフレームを足掛かりとすることで自己位置推定処理の高速化を図ることができる。
カメラから取得された視界画像によって観測された特徴点と、地図上に記録された特徴点との整合数が少ない（つまり、取得した視界画像に、地図上に記録された特徴点がほとんど映っていない）場合などにおいては、自己位置推定処理に失敗し、自己の位置及び姿勢を見失う（ロストする）ことが知られている。ＯＲＢ−ＳＬＡＭにおいては、自己の位置及び姿勢を見失うことなく、安定した自己位置推定処理を行うことが求められている。

本開示の少なくとも一態様は、カメラを用いた自己位置推定処理の安定化を図ることができる自己位置推定装置、自己位置推定方法及びプログラムを提供することを目的とする。

本発明の第１の態様によれば、自己位置推定装置は、移動体に搭載されたカメラから視界画像を取得する取得部と、前記視界画像上における特徴点の位置と、世界座標上における複数の特徴点の位置が記録されてなる地図とに基づいて、前記世界座標上における前記移動体の位置及び姿勢を推定する自己位置推定処理を行う推定部と、前記視界画像を、当該視界画像上に含まれる新たな特徴点を前記地図に追加するためのキーフレームとして登録する登録部と、を備える。前記登録部は、前記移動体の移動が、当該移動体の位置及び姿勢の少なくともいずれかについて予め規定された条件を満たした場合に、前記視界画像を前記キーフレームとして登録する。

また、本発明の第２の態様によれば、前記登録部は、前記移動体の位置と、当該位置に紐づけられる１つの前記キーフレームとの距離が所定の距離閾値以上離れている場合に、前記視界画像をキーフレームとして登録する。

また、本発明の第３の態様によれば、前記登録部は、移動中の前記移動体の角速度が所定の角速度閾値以上となった場合に、前記視界画像をキーフレームとして登録する。

また、本発明の第４の態様によれば、前記登録部は、消去防止フラグの有無に基づいて、登録済の複数の前記キーフレームに対する間引き処理を行う。前記登録部は、複数の前記キーフレームの１つである対象キーフレームに対し、当該対象キーフレームよりも１つ前に登録されたキーフレームと、当該対象キーフレームよりも１つ後に登録されたキーフレームとの位置及び姿勢の少なくともいずれかの関係に基づいて、当該対象キーフレームに前記消去防止フラグを付するか否かを判定する。

また、本発明の第５の態様によれば、上述の自己位置推定装置は、前記地図のうちの一部の領域をメモリ上に読み出す読出部を更に備える。

また、本発明の第６の態様によれば、前記読出部は、前記地図のうち、前記移動体の位置を基準とする前記一部の領域をメモリ上に読み出す。

また、本発明の第７の態様によれば、前記読出部は、前記地図のうち、前記移動体の幅方向と比較して前記移動体の前後方向に広い前記一部の領域をメモリ上に読み出す。

また、本発明の第８の態様によれば、前記読出部は、前記地図のうち、特定の走行ルートに対応する前記一部の領域をメモリ上に読み出す。

また、本発明の第９の態様によれば、上述の自己位置推定装置は、前記自己位置推定処理に失敗した場合に前記キーフレームを探索するリローカリゼーションを行う処理部を備える。前記処理部は、前記移動体のハードウェアの制約条件に基づいて探索範囲を特定するとともに、当該探索範囲に含まれるキーフレームを探索する。

また、本発明の第１０の態様によれば、前記処理部は、前記自己位置推定処理で推定された位置と、前記移動体の最大速度とに基づいて、当該移動体の現在の位置としてとり得る範囲を前記探索範囲として特定する。

また、本発明の第１１の態様によれば、前記処理部は、前記自己位置推定処理で推定された姿勢と、前記移動体の最大角速度とに基づいて、当該移動体の現在の姿勢としてとりうる範囲を前記探索範囲として特定する。

また、本発明の第１２の態様によれば、前記処理部は、前記自己位置推定処理で推定された位置を基準とする、オドメトリによる推定位置の誤差とみなせる範囲を前記探索範囲として特定する。

また、本発明の第１３の態様によれば、前記処理部は、前記自己位置推定処理で推定された姿勢を基準とする、ジャイロセンサによる推定姿勢の誤差とみなせる範囲を前記探索範囲として特定する。

また、本発明の第１４の態様によれば、前記推定部は、前記世界座標上における前記移動体の位置及び姿勢の軌跡が平滑化されるように、前記自己位置推定処理によって推定された位置及び姿勢を修正する。

また、本発明の第１５の態様によれば、前記推定部は、移動平均フィルタを用いて、前記自己位置推定処理によって推定された位置及び姿勢を修正する。

また、本発明の第１６の態様によれば、自己位置推定方法は、移動体に搭載されたカメラから視界画像を取得するステップと、前記視界画像上における特徴点の位置と、世界座標上における複数の特徴点の位置が記録されてなる地図とに基づいて、前記世界座標上における前記移動体の位置及び姿勢を推定する自己位置推定処理を行うステップと、前記視界画像を、当該視界画像上に含まれる新たな特徴点を前記地図に追加するためのキーフレームとして登録するステップと、を有する。前記キーフレームとして登録するステップでは、前記移動体の移動が、当該移動体の位置及び姿勢の少なくともいずれかについて規定された条件を満たした場合に、前記視界画像を前記キーフレームとして登録する。

また、本発明の第１７の態様によれば、プログラムは、自己位置推定装置のコンピュータに、移動体に搭載されたカメラから視界画像を取得するステップと、前記視界画像上における特徴点の位置と、世界座標上における複数の特徴点の位置が記録されてなる地図とに基づいて、前記世界座標上における前記移動体の位置及び姿勢を推定する自己位置推定処理を行うステップと、前記視界画像を、当該視界画像上に含まれる新たな特徴点を前記地図に追加するためのキーフレームとして登録するステップと、を実行させる。前記キーフレームとして登録するステップでは、前記移動体の移動が、当該移動体の位置及び姿勢の少なくともいずれかについて規定された条件を満たした場合に、前記視界画像を前記キーフレームとして登録する。

上述の発明の各態様によれば、カメラを用いた自己位置推定処理の安定化を図ることができる。

第１の実施形態に係るフォークリフトの全体構成を示す図である。 第１の実施形態に係る自己位置推定装置の機能構成を示す図である。 第１の実施形態に係るキーフレームテーブルの例を示す図である。 第１の実施形態に係る地図の例を示す図である。 第１の実施形態に係る変換行列を説明する図である。 第１の実施形態に係る自己位置推定装置による地図作成時の処理フローを示す図である。 第１の実施形態に係る自己位置推定装置による実運用時の処理フローを示す図である。 第１の実施形態の変形例に係る自己位置推定装置による地図作成時の処理フローを示す図である。 第２の実施形態に係る地図読出部の処理を説明する図である。 第２の実施形態の変形例に係る地図読出部の処理を説明する図である。 第３の実施形態に係るリローカリゼーション処理部の処理フローを示す図である。 第３の実施形態に係るリローカリゼーション処理部の処理を説明する図である。 第４の実施形態に係る自己位置推定処理部の機能を説明する図である。

＜第１の実施形態＞
以下、第１の実施形態に係る自己位置推定装置、及び、これを備える移動体について、図１〜図７を参照しながら説明する。

（フォークリフトの全体構成）
図１は、第１の実施形態に係るフォークリフトの全体構成を示す図である。
図１に示すフォークリフト１は、移動体の一態様であり、予め定められたルートに沿って目標地点まで自律走行する無人フォークリフトである。

フォークリフト１は、自己位置推定装置１０と、カメラ１１と、タイヤ回転角センサ１２と、舵角センサ１３と、ジャイロセンサ１４とを備えている。

自己位置推定装置１０は、ＯＲＢ−ＳＬＡＭによる自己位置推定処理を行う。具体的には、カメラ１１から取得される画像（以下、視界画像とも表記する。）に基づいて、フォークリフト１の位置及び姿勢を推定する。自己位置推定装置１０の詳細な構成及び機能については後述する。

カメラ１１は、動画撮影、即ち、連続的に視界画像を取得可能な撮影装置である。視界画像の１つ１つは、動画を構成する“フレーム”に相当する。本実施形態に係るカメラ１１は、「単眼」であるものとして説明するが、他の実施形態においては「複眼」であってもよい。

タイヤ回転角センサ１２は、フォークリフト１のタイヤ回転角を検出可能なセンサである。

舵角センサ１３は、タイヤの舵角を検出可能なセンサである。上述のタイヤ回転角センサ１２によるタイヤ回転角の検出結果と舵角センサ１３による舵角の検出結果とを組み合わせることで、フォークリフト１の、ある位置から移動した後の相対的な位置を特定することができる。

ジャイロセンサ１４は、フォークリフト１の回転角速度を検出可能なセンサである。

また、本実施形態に係るフォークリフト１には、フォークリフト１の実運用開始前に行われる地図作成時において、レーザスキャナが搭載される（図１には不図示）。このレーザスキャナは、フォークリフト１の実運用時（作成済みの地図を用いたＯＲＢ−ＳＬＡＭによる自己位置推定処理を行いながらの走行時）には取り外される。
このレーザスキャナは、ＬｉＤＡＲに利用されるセンサであって、広範囲に渡って走査されるレーザの反射に基づき、レーザスキャナの周囲に配置された構造物との間隔を測距可能なセンサである。レーザスキャナによって得られた広範囲に渡る測距を利用したＳＬＡＭであるＬｉＤＡＲ−ＳＬＡＭにより、ＯＲＢ−ＳＬＡＭによる自己位置推定処理よりも高い精度でフォークリフト１の位置及び姿勢を推定することができる。

また、本実施形態に係るフォークリフト１は、自己位置推定装置１０が出力する、ＯＲＢ−ＳＬＡＭによる自己位置推定処理の結果に基づいて操舵制御を行う操舵制御装置を備えている（図１には不図示）。この操舵制御装置は、例えば、自己位置推定装置１０によって推定されたフォークリフト１の位置及び姿勢に基づくフィードバック制御により操舵制御を行う。

（フォークリフトの機能構成）
図２は、第１の実施形態に係る自己位置推定装置の機能構成を示す図である。
図２に示すように、自己位置推定装置１０は、ＣＰＵ１００と、メモリ１０１と、ストレージ１０２と、通信インタフェース１０３とを備えている。
ＣＰＵ１００は、自己位置推定装置１０の動作全体を司るプロセッサであって、予め用意されたプログラムに従って動作することで種々の機能を発揮する。
メモリ１０１は、例えばＤＲＡＭ等いわゆる主記憶装置であって、ＣＰＵ１００がプログラムに従って高速で動作するために必要な記憶領域である。
ストレージ１０２は、例えばＨＤＤ、ＳＳＤ等のいわゆる補助記憶装置であって、自己位置推定装置１０の処理に必要な情報が予め記録されている。
通信インタフェース１０３は、カメラ１１、タイヤ回転角センサ１２、舵角センサ１３、ジャイロセンサ１４等と通信するための接続インタフェースである。

ＣＰＵ１００の機能について詳しく説明する。
ＣＰＵ１００は、予め用意されたプログラムに従って動作することで、視界画像取得部１０００（取得部）、自己位置推定処理部１００１（推定部）、キーフレーム登録部１００２（登録部）、地図読出部１００３（読出部）、及び、リローカリゼーション処理部１００４（処理部）としての機能を発揮する。

視界画像取得部１０００は、フォークリフト１に搭載されたカメラ１１から、動画の各フレームとしての視界画像を逐次取得する。

自己位置推定処理部１００１は、ＯＲＢ−ＳＬＡＭによる自己位置推定処理を行う。具体的には、自己位置推定処理部１００１は、カメラ１１から取得された視界画像上における特徴点の位置と、世界座標上における複数の特徴点の位置が記録されてなる地図Ｍとに基づいて、世界座標上におけるフォークリフト１（カメラ１１）の位置及び姿勢を推定する。

キーフレーム登録部１００２は、地図作成時において、カメラ１１から取得された視界画像のうち特定の条件を満たしたものをキーフレームとして登録する。キーフレームは、その視界画像（フレーム）によって観測された特徴点を地図Ｍに追加するためのフレームとして選定される。

地図読出部１００３は、地図Ｍをメモリ１０１上に読み出す。

リローカリゼーション処理部１００４は、自己位置推定処理部１００１のＯＲＢ−ＳＬＡＭによる自己位置推定処理に失敗した場合に、リローカリゼーションを行う。リローカリゼーションとは、現時点で取得された視界画像と似たキーフレームが存在しないかどうかを探索して復帰を図る処理である。

（キーフレームテーブルの例）
図３は、第１の実施形態に係るキーフレームテーブルの例を示す図である。
キーフレーム登録部１００２は、地図作成時において、所定の条件を満たした視界画像を、キーフレームとして、キーフレームテーブルに登録する。
図３に示すように、キーフレームテーブルは、「キーフレーム番号」、「視界画像」、「画像上の特徴点の位置」、「対応する地図上の特徴点」、「移動体の位置、姿勢」及び「変換行列」が関連付けられて記録される。
「キーフレーム番号」は、キーフレームを特定するために割り振られる識別番号である。
「視界画像」は、視界画像そのものの画像データである。なお、他の実施形態においては視界画像の画像データが記録される態様に限定されず、画像データよりも容量が小さく、かつ、画像の類似度を測るのに適した別の形式で記録される態様であってもよい。例えば、ＢｏＷ（Bag of Words）と呼ばれる、特徴ベクトルの統計情報であってもよい。
「画像上の特徴点の位置」は、視界画像よって観測された特徴点の画像上の位置（ピクセル座標（ｐｘ、ｐｙ）で特定される位置）である。図３に示すキーフレームテーブルによれば、例えば、キーフレーム番号“ｎ”のキーフレーム（視界画像）からは、特徴点ｑ１＿ｎ、ｑ２＿ｎ、ｑ３＿ｎ、・・が観測され、それぞれ、視界画像上の位置（ｐｘ、ｐｙ）で特定されている。
「対応する地図上の特徴点」は、キーフレームによって観測された特徴点に対応する地図Ｍ上の特徴点を示す。図３に示すキーフレームテーブルによれば、例えば、キーフレーム番号“ｎ”のキーフレームによって観測された特徴点ｑ１＿ｎ、ｑ２＿ｎ、ｑ３＿ｎ、・・は、地図Ｍの世界座標上にプロットされた特徴点Ｑ２、Ｑ４、Ｑ５、・・にそれぞれ対応する。地図Ｍ上の各特徴点Ｑ２、Ｑ４、Ｑ５、・・は、それぞれ、世界座標値（Ｘ，Ｙ，Ｚ）で規定される。
「移動体の位置、姿勢」は、各キーフレームを取得した際のフォークリフト１（カメラ１１）の地図Ｍ上の位置及び姿勢が記録される。図３に示すキーフレームテーブルには、例えば、キーフレーム番号“ｎ”のキーフレームを取得した際のフォークリフト１の位置Ｐｎ（Ｘ，Ｙ，Ｚ）及び姿勢Ａｎ（θｘ，θｙ，θｚ）が記録されている。なお、後述するように、ここに記録されるフォークリフト１の位置Ｐｎ（Ｘ，Ｙ，Ｚ）及び姿勢Ａｎ（θｘ，θｙ，θｚ）は、ＯＲＢ−ＳＬＡＭによる自己位置推定処理によって推定された位置及び姿勢ではなく、ＬｉＤＡＲ−ＳＬＡＭによる自己位置推定処理によって推定された位置及び姿勢が記録される。したがって、ＯＲＢ−ＳＬＡＭによる自己位置推定処理の結果よりも精度の高い結果として得られる。以下の説明においては、この項に記録された位置及び姿勢を、フォークリフト１の位置及び姿勢の真値として扱う。
「変換行列」は、ＯＲＢ−ＳＬＡＭによる自己位置推定処理の結果を、真値に変換するための変換行列である。変換行列は、各キーフレームに紐づけて記録される。図３に示すキーフレームテーブルには、例えば、キーフレーム番号“ｎ”のキーフレームに紐づく変換行列Ｔｎが記録されている。この変換行列Ｔｎは、キーフレーム番号“ｎ”のキーフレームを取得した際のフォークリフト１のＯＲＢ−ＳＬＡＭによる自己位置推定処理の結果の、真値からの偏差（ずれ）を補正するように算出された変換行列である。

（地図の例）
図４は、第１の実施形態に係る地図の例を示す図である。
地図Ｍは、キーフレームテーブル（図３）に基づいて作成され、図４に示すような、世界座標を基準とする各種情報群として記録される。
地図作成時において、自己位置推定装置１０は、走行ルートＲの走行中にカメラ１１から取得される視界画像のうち、所定の条件を満たしたものをキーフレームＫＦとしてキーフレームテーブル（図３）に登録する。ここで、走行ルートＲは、例えば、フォークリフト１の実運用時において走行することが予定されている走行ルートである。
図４に示すように、地図Ｍには、各キーフレームＫＦ取得時のフォークリフト１の位置及び姿勢の真値である位置Ｐ（Ｘ，Ｙ，Ｚ）及び姿勢Ａ（θｘ，θｙ，θｚ）と、各キーフレームＫＦによって観測された特徴点Ｑの世界座標上の位置（Ｘ，Ｙ，Ｚ）とが記録される。図３で説明したように、世界座標上にプロットされた各特徴点Ｑは、当該特徴点Ｑを観測したキーフレームＫＦと紐づけられている。なお、図４に示す構造物Ｇは、キーフレームＫＦに映り込む構造物である。各特徴点Ｑは、キーフレームＫＦに映り込んだ構造物Ｇ等に基づいて観測される。

（変換行列）
図５は、第１の実施形態に係る変換行列を説明する図である。
図３に示すキーフレームテーブルの「変換行列」（Ｔｎ−１、Ｔｎ、Ｔｎ＋１、・・）は、以下のようにして求められる。
図５において、ある時点に取得された視界画像をキーフレームＫＦｎ−１としてキーフレームテーブル（図３）に登録する場合の処理を考える。この場合、自己位置推定装置１０は、当該視界画像取得時のフォークリフト１の位置及び姿勢を、作成中の地図Ｍに基づくＯＲＢ−ＳＬＡＭにより推定する。図５において、この推定結果を位置Ｐｎ−１’、姿勢Ａｎ−１’とする。他方、自己位置推定装置１０は、地図作成時に搭載されているレーザスキャナを通じて、キーフレームＫＦｎ−１取得時のフォークリフト１の位置及び姿勢の真値を取得することができる。図５において、フォークリフト１の位置及び姿勢の真値を位置Ｐｎ−１、姿勢Ａｎ−１とする。自己位置推定装置１０は、キーフレームＫＦｎ−１を登録する際、位置Ｐｎ−１’、姿勢Ａｎ−１’を真値である位置Ｐｎ−１、姿勢Ａｎ−１に写像するための変換行列Ｔｎ−１を算出し、これをキーフレームＫＦｎ−１に紐づけてキーフレームテーブルに記録する。
自己位置推定装置１０は、新たなキーフレームＫＦｎ、ＫＦｎ＋１、・・を登録する度に、各キーフレームＫＦｎ、ＫＦｎ＋１、・・に対応する変換行列Ｔｎ、Ｔｎ＋１、・・を算出し、キーフレームテーブルに記録する。

（地図作成時の処理フロー）
図６は、第１の実施形態に係る自己位置推定装置による地図作成時の処理フローを示す図である。
図６に示す処理フローは、地図作成時におけるフォークリフト１の走行中に繰り返し実行される。この時点では、作成途中の地図Ｍが既にメモリ１０１上に読み込まれているものとする。

自己位置推定装置１０の視界画像取得部１０００は、カメラ１１から視界画像を取得する（ステップＳ００）。
自己位置推定装置１０の自己位置推定処理部１００１は、ステップＳ００で取得された視界画像と、作成途中の地図Ｍとに基づいてＯＲＢ−ＳＬＡＭによる自己位置推定処理を行う。ここで、自己位置推定処理部１００１は、ステップＳ００で取得された視界画像によって観測された特徴点と、地図Ｍ上に記録された特徴点とを整合（マッチング）することで、その視界画像取得時のカメラの世界座標上における位置及び姿勢を推定する。この際、自己位置推定処理部１００１は、ステップＳ００で取得された視界画像に対し、特徴点の位置関係が最も近いキーフレーム（カレントキーフレーム）を１つ特定する。そして、自己位置推定処理部１００１は、このカレントキーフレーム、及び、当該カレントキーフレームと時系列で前後する他のキーフレームに紐づけられた地図上の特徴点を参照しながら、視界画像によって観測された特徴点と整合するものを検索する。このようにすることでＯＲＢ−ＳＬＡＭによる自己位置推定処理の高速化を図ることができる。

自己位置推定処理部１００１は、ＯＲＢ−ＳＬＡＭによる自己位置推定処理が成功したか否かを判定する（ステップＳ０２）。
ＯＲＢ−ＳＬＡＭによる自己位置推定処理が成功した場合（ステップＳ０２；ＹＥＳ）、ステップＳ００で取得した視界画像をキーフレームとして登録すべきか否かの判定処理（ステップＳ０５〜ステップＳ０７）に進む。当該処理の詳細については後述する。
ＯＲＢ−ＳＬＡＭによる自己位置推定処理が失敗した場合（ステップＳ０２；ＮＯ）、自己位置推定装置１０のリローカリゼーション処理部１００４は、リローカリゼーションを行う（ステップＳ０３）。本実施形態に係るリローカリゼーション処理部１００４は、キーフレームテーブルに登録されている全てのキーフレームを対象に探索を行う。
リローカリゼーション処理部１００４は、ステップＳ０３のリローカリゼーションに成功したか否かを判定する（ステップＳ０４）。リローカリゼーションに失敗した場合（ステップＳ０４；ＮＯ）、リローカリゼーション処理部１００４は、ステップＳ０４に戻り、リローカリゼーションを引き続き実行する。リローカリゼーションにより位置及び姿勢を推定できた（成功した）場合（ステップＳ０４；ＹＥＳ）、自己位置推定装置１０は上記判定処理（ステップＳ０５〜ステップＳ０７）に進む。

自己位置推定装置１０のキーフレーム登録部１００２は、以下に説明するステップＳ０５〜ステップＳ０７の判定処理により、ステップＳ００で取得された視界画像をキーフレームとして登録するか否かを判定する。

具体的には、まず、キーフレーム登録部１００２は、ステップＳ００で取得された視界画像によって観測された特徴点と整合する地図Ｍ上の特徴点を検索する。次に、キーフレーム登録部１００２は、ステップＳ００で取得された視界画像によって観測された特徴点のうち新たな特徴点（地図Ｍ上に記録されていない特徴点）が所定数以上観測されたか否かを判定する（ステップＳ０５）。
ステップＳ００で取得された視界画像に新たな特徴点が所定数以上観測された場合（ステップＳ０５；ＹＥＳ）、キーフレーム登録部１００２は、ステップＳ００で取得された視界画像をキーフレームとして登録する（ステップＳ０８）。

ステップＳ００で取得された視界画像に新たな特徴点が所定数以上観測されなかった場合（ステップＳ０５；ＮＯ）、次に、キーフレーム登録部１００２は、ステップＳ０１のＯＲＢ−ＳＬＡＭによる自己位置推定処理（又は、ステップＳ０３のリローカリゼーションの結果）で推定された位置がカレントキーフレームの位置から所定の距離閾値以上離れているか否かを判定する（ステップＳ０６）。
ＯＲＢ−ＳＬＡＭによる自己位置推定処理で推定された位置がカレントキーフレームの位置から所定の距離閾値以上離れていた場合（ステップＳ０６；ＹＥＳ）、キーフレーム登録部１００２は、ステップＳ００で取得された視界画像をキーフレームとして登録する（ステップＳ０８）。

ＯＲＢ−ＳＬＡＭによる自己位置推定処理で推定された位置がカレントキーフレームの位置から所定の距離閾値以上離れていなかった場合（ステップＳ０６；ＮＯ）、次に、キーフレーム登録部１００２は、ジャイロセンサ１４による角速度の検出結果を参照して、視界画像取得時におけるフォークリフト１の角速度が所定の角速度閾値以上となっているか否かを判定する（ステップＳ０７）。
視界画像取得時におけるフォークリフト１の角速度が所定の角速度閾値以上となっていた場合（ステップＳ０７；ＹＥＳ）、キーフレーム登録部１００２は、ステップＳ００で取得された視界画像をキーフレームとして登録する（ステップＳ０８）。

視界画像取得時におけるフォークリフト１の角速度が所定の角速度閾値以上となっていなかった場合（ステップＳ０７；ＮＯ）、キーフレーム登録部１００２は、ステップＳ００で取得された視界画像をキーフレームとして登録しないで、次の段階の処理フローに移行する。

ステップＳ０８において、キーフレーム登録部１００２は、ステップＳ０１のＯＲＢ−ＳＬＡＭによる自己位置推定結果ではなく、ＬｉＤＡＲ−ＳＬＡＭによる自己位置推定結果（真値）をキーフレームテーブルに記録する。更に、キーフレーム登録部１００２は、ＬｉＤＡＲ−ＳＬＡＭによる自己位置推定結果と、ステップＳ０１のＯＲＢ−ＳＬＡＭによる自己位置推定結果とを用いて変換行列を算出し、当該変換行列をキーフレームテーブルに記録する。
また、キーフレーム登録部１００２は、キーフレームとしての登録後、更に、バンドル処理等の後処理を実施する。

（実運用時の処理フロー）
図７は、第１の実施形態に係る自己位置推定装置による実運用時の処理フローを示す図である。
地図完成後の実運用時において、自己位置推定装置１０は、以下に示すステップＳａ０〜ステップＳａ３の流れで、ＯＲＢ−ＳＬＡＭによる自己位置推定処理を行う。

まず、視界画像取得部１０００は、カメラ１１から視界画像Ｆａを取得する（ステップＳａ０）。
次に、自己位置推定処理部１００１は、視界画像Ｆａによって観測された特徴点と、作成済みの地図Ｍとに基づいてＯＲＢ−ＳＬＡＭによる自己位置推定処理を行う（ステップＳａ１）。この処理で推定されたフォークリフト１の位置及び姿勢をそれぞれ、位置Ｐａ’、姿勢Ａａ’とする。
次に、自己位置推定処理部１００１は、キーフレームテーブルに登録されているキーフレームのうち、ステップＳａ０で取得された視界画像Ｆａと、特徴点の位置関係が最も近いキーフレームであるカレントキーフレームを特定する（ステップＳａ２）。ここで特定されたカレントキーフレームが、キーフレーム番号“ｎ”のキーフレーム（キーフレームＫＦｎ）であったとする。
次に、自己位置推定装置１０は、ＯＲＢ−ＳＬＡＭによって推定された位置Ｐａ’、姿勢Ａａ’に対し、カレントキーフレーム（キーフレームＫＦｎ）に紐づく変換行列Ｔｎを乗じて、最終的な位置Ｐａ及び姿勢Ａａを演算する。

（作用、効果）
以上の通り、第１の実施形態に係る自己位置推定装置１０は、例えば、図６のステップＳ０６、ステップＳ０７の処理のように、フォークリフト１の移動が、当該フォークリフト１の位置及び姿勢の少なくともいずれかについて予め規定された条件を満たした場合に、視界画像をキーフレームとして登録することを特徴とする。

ここで、図６に示すステップＳ０６及びステップＳ０７の判定処理を行わず、ステップＳ０５の判定処理のみを行う場合について生じる課題について説明する。
即ち、ステップＳ０５の判定処理は、視界画像において地図Ｍ上に記録されていない新たな特徴点が所定数以上観測されたか否かに基づいて、当該視界画像をキーフレームとして登録するか否かを判定するものである。この場合、例えば、カメラ１１が向く方向に直進する場合などにおいては、前回のキーフレームと比較して新たな特徴点が観測されにくくなるため、キーフレームの間隔が疎になりやすい。キーフレームの間隔が疎になると地図Ｍ上に記録される特徴点の数も減るため、ロストしやすくなることが想定される。
また、フォークリフト１の旋回移動時には短時間で視界が大きく変わるため、ロストしやすくなることが知られている。そのため、安定した自己位置推定を実現するためには、特に、フォークリフト１の旋回位置周辺において充分に密な間隔でキーフレームが登録されていることが望まれる。
更に、キーフレームの間隔が疎になると、カレントキーフレームとの距離が離れてしまうため、各キーフレームに紐づく変換行列による自己位置推定処理の補正精度も低下する。したがって、変換行列による補正精度を高める観点からも、適切な密度でキーフレームが登録されていることが好ましい。

第１の実施形態に係る自己位置推定装置１０によれば、フォークリフト１の移動が、当該フォークリフト１の位置及び姿勢の少なくともいずれかについて予め規定された条件を満たした場合に、視界画像をキーフレームとして登録する。
このようにすることで、例えば、キーフレームが疎となりやすい直進時や、キーフレームをより密とすべき旋回時など、条件に応じた特定の移動態様が発生した場合に、従来よりもキーフレームが登録されやすくなる。したがって、フォークリフト１の移動に応じた適切な間隔でキーフレームが挿入されることとなり、カメラを用いた自己位置推定処理の安定化を図ることができる。

また、第１の実施形態に係る自己位置推定装置１０によれば、フォークリフト１の位置について予め規定された条件の一態様として、フォークリフト１の位置と、当該位置に紐づけられる１つのキーフレーム（カレントキーフレーム）との距離が所定の距離閾値以上離れている場合に、視界画像をキーフレームとして登録する（図６のステップＳ０６）。
このようにすることで、特に直進時など、新たな特徴点が観測されにくい状況にあったとしても、距離判定閾値以下の間隔でキーフレームが登録されることとなり、キーフレームが疎となることを抑制することができる。

また、第１の実施形態に係る自己位置推定装置１０によれば、フォークリフト１の姿勢について予め規定された条件の一態様として、移動中のフォークリフト１の角速度が所定の角速度閾値以上となった場合に、視界画像をキーフレームとして登録する（図６のステップＳ０７）。
このようにすることで、旋回角速度が所定の角速度閾値以上となった場合には、その分だけ密にキーフレームが登録されることとなり、旋回時にロストすることを抑制することができる。

なお、上述したステップＳ０５〜ステップＳ０７の判定処理の順番は適宜入れ替え可能であり、他の実施形態においては図６に示す順番に限定されない。また、他の実施形態においては、ステップＳ０５〜ステップＳ０７の全てを行う必要はなく、ステップＳ０６及びステップＳ０７の少なくともいずれか一方を実施することで上記効果を奏することができる。

（第１の実施形態の変形例）
ＯＲＢ−ＳＬＡＭによる地図作成時において、あるキーフレームに含まれる特徴点が十分多くの他のキーフレームによって観測されている場合、データ量削減の観点から、そのキーフレームを削除する間引き処理を行う場合がある。この間引き処理によって、キーフレームの密度が低減してしまうことを防止するため、本実施形態の変形例に係る自己位置推定装置１０は、地図作成時において、図８に示すような処理を行ってもよい。

（地図作成時の処理フロー）
図８は、第１の実施形態の変形例に係る自己位置推定装置による地図作成時の処理フローを示す図である。

地図作成時において、キーフレーム登録部１００２は、ｎ番目のキーフレームを登録する（ステップＳ１０）。このとき、キーフレーム登録部１００２は、ｎ番目のキーフレームに対し、ポインタとして「ｌａｔｅｓｔ」を割り当てるとともに、当該ｎ番目のキーフレームに削除防止フラグを付する。なお、この時点で、ｎ−１番目のキーフレームには、当該キーフレームを指し示すポインタとして「ｐｒｅｖ１」が割り当てられ、ｎ−２番目のキーフレームには、当該キーフレームを指し示すポインタとして「ｐｒｅｖ２」が割り当てられているものとする。また、ｎ−１番目のキーフレーム及びｎ−２番目のキーフレームにも、それぞれ削除防止フラグが付されているものとする。

次に、キーフレーム登録部１００２は、「ｐｒｅｖ２」で指し示されるキーフレームの位置と「ｌａｔｅｓｔ」で指し示されるキーフレームの位置との距離が所定距離以上か否かを判定する（ステップＳ１１）。
「ｐｒｅｖ２」で指し示されるキーフレームの位置と「ｌａｔｅｓｔ」で指し示されるキーフレームの位置との距離が所定距離未満であった場合（ステップＳ１１；ＮＯ）、次に、キーフレーム登録部１００２は、「ｐｒｅｖ２」で指し示されるキーフレームの姿勢と「ｌａｔｅｓｔ」で指し示されるキーフレームの姿勢との角度が所定角度以上か否かを判定する（ステップＳ１２）。
「ｐｒｅｖ２」で指し示されるキーフレームの姿勢と「ｌａｔｅｓｔ」で指し示されるキーフレームの姿勢との角度が所定角度未満であった場合（ステップＳ１２；ＮＯ）、次に、キーフレーム登録部１００２は、カレントキーフレームが「ｐｒｅｖ１」で指し示されるキーフレームか否かを判定する（ステップＳ１３）。

カレントキーフレームが「ｐｒｅｖ１」で指し示されるキーフレームであった場合（ステップＳ１３；ＹＥＳ）、「ｐｒｅｖ２」と「ｌａｔｅｓｔ」との間の距離及び角度の差異が小さく、また、「ｌａｔｅｓｔ」のキーフレームと関係が近いカレントキーフレームが「ｐｒｅｖ１」であることから、「ｐｒｅｖ１」で指し示されるキーフレームは、間引かれたとしても大きな影響（ロスト発生等）を及ぼす可能性は低い。
したがって、上記の３つの条件の全てを満たした場合（ステップＳ１１；ＮＯ、ステップＳ１２；ＮＯ、かつ、ステップＳ１３；ＹＥＳ）、自己位置推定装置１０は、「ｐｒｅｖ１」のキーフレームに付されている削除防止フラグを削除し、「ｌａｔｅｓｔ」で指し示されるキーフレームのポインタを「ｐｒｅｖ１」に置き換える（ステップＳ１４）。
この後、キーフレーム登録部１００２は、次のキーフレームを登録し、当該キーフレームに「ｌａｔｅｓｔ」のポインタを割り当てる（ステップＳ１５）。

他方、上記の３つの条件のいずれか１つでも満たさなかった場合（ステップＳ１１；ＹＥＳ、ステップＳ１２；ＹＥＳ、又は、ステップＳ１３；ＮＯ）、「ｐｒｅｖ１」に示されるキーフレームが間引かれたときには、「ｐｒｅｖ２」のキーフレームと「ｌａｔｅｓｔ」のキーフレームとの距離（又は角度）の間隔が大きく開いてしまい、大きな影響（ロスト発生等）を及ぼす可能性があると考えられる。そこで、この場合、キーフレーム登録部１００２は、「ｐｒｅｖ１」の削除防止フラグを削除することなく、「ｌａｔｅｓｔ」で指し示されるキーフレームのポインタを「ｐｒｅｖ１」に置き換え、「ｐｒｅｖ１」で指し示されるキーフレームのポインタを「ｐｒｅｖ２」に置き換える（ステップＳ１６）。
この後、キーフレーム登録部１００２は、次のキーフレームを登録し、当該キーフレームに「ｌａｔｅｓｔ」のポインタを割り当てる（ステップＳ１７）。

キーフレーム登録部１００２は、ステップＳ１１〜ステップＳ１７の処理を、地図作成の処理が終了するまで繰り返し実行する（ステップＳ１８）。

以上のように、第1の実施形態の変形例に係るキーフレーム登録部１００２は、消去防止フラグの有無に基づいて、登録済の複数のキーフレームに対する間引き処理を行う。
そして、キーフレーム登録部１００２は、消去防止フラグを付するか否かの判断を行う対象キーフレーム（ｐｒｅｖ１）に対し、当該対象キーフレームよりも１つ前に登録されたキーフレーム（ｐｒｅｖ２）と、当該対象キーフレームよりも１つ後に登録されたキーフレーム（ｌａｔｅｓｔ）との位置及び姿勢の関係に基づいて、当該対象キーフレームに消去防止フラグを付するか否かを判定する。
このようにすることで、キーフレーム密度に大きな影響を及ぼす可能性のあるキーフレームに削除防止フラグが設定されるので、間引き処理を行った後においても、フォークリフト１の位置、姿勢に応じた適切なキーフレームの密度を確保することができる。

なお、上述したステップＳ１１〜ステップＳ１３の判定処理の順番は適宜入れ替え可能であり、他の実施形態においては図８に示す順番に限定されない。また、他の実施形態においては、ステップＳ１１〜ステップＳ１３の全てを行う必要はなく、ステップＳ１１〜ステップＳ１３の少なくともいずれか一方を実施することで上記効果を奏することができる。

＜第２の実施形態＞
以下、第２の実施形態に係る自己位置推定装置、及び、これを備える移動体について、図９、図１０を参照しながら説明する。

（地図読出部の処理）
図９は、第２の実施形態に係る地図読出部の処理を説明する図である。
第１の実施形態に係る地図読出部１００３は、実運用時において、メモリ１０１上に地図Ｍの全てをロードする（読み出す）が、第２の実施形態に係る地図読出部１００３は、地図Ｍの一部の領域をメモリ上に読み出す点で、第１の実施形態と相違する。

第２の実施形態に係る地図読出部１００３は、実運用時において、ＯＲＢ−ＳＬＡＭによる自己位置推定処理で推定されたフォークリフト１の位置を基準とする所定の領域（読出領域ＭＲ）に属するキーフレームのみをメモリ１０１上にロードする。図９において、地図読出部１００３は、フォークリフト１の位置が移動する度に、そのときの位置（撮影画像Ｆｃに基づいて推定される位置）を基準とする新たな読出領域ＭＲを規定し、当該読出領域ＭＲに属するキーフレームＫＦａをメモリ１０１上にロードする。そして、地図読出部１００３は、当該読出領域ＭＲに属さないキーフレームＫＦｂをメモリ１０１上からアンロードする。

このようにすることで、フォークリフト１の現在位置に応じた、自己位置推定処理に必要な最小限のキーフレームのみがメモリ１０１上にロードされるので、自己位置推定処理の精度を維持しながらもメモリ１０１の使用領域を節約することができる。

また、第２の実施形態に係る読出し領域ＭＲは、図９に示すように、フォークリフト１の幅方向（図９に示す例ではＹ軸方向）と比較してフォークリフト１の前後方向（図９に示す例ではＸ軸方向）に広くなるような形状（図９に示すような、フォークリフトの現在位置を中心とする円形に対し、フォークリフト１の前後方向に扇形の形状を組み合わせた形状）とされている。ここで、フォークリフト１は、前後方向には短時間で到達する可能性があるところ、上記のような形状とすることで、前後方向に多くの特徴点がロードされる。したがって、フォークリフト１が前後方向に移動する場合に、地図Ｍ上で整合する特徴点が不足することを防止することができる。

なお、他の実施形態においては、読出領域ＭＲは、上記のような形状に限定される必要はなく、単に、フォークリフト１の位置を基準（中心）とする円状又は矩形状に規定される態様であってもよい。

（第２の実施形態の変形例）
図１０は、第２の実施形態に係る地図読出部の処理を説明する図である。
第２の実施形態の変形例に係る地図読出部１００３は、実運用時において、特定の走行ルートに対応する読出領域ＭＲに属するキーフレームのみをメモリ１０１上にロードする。図１０において、読出領域ＭＲは、走行ルートＲに対応して規定されたものである。フォークリフト１が走行予定とする走行ルート（例えば走行ルートＲ）が変更された場合、地図読出部１００３は、読出領域ＭＲを、変更後の走行ルート（例えばルートＲ’）に対応するものに変更する。地図読出部１００３は、第２の実施形態と同様に、読出領域ＭＲに属するキーフレームＫＦａをメモリ１０１上にロードし、当該読出領域ＭＲに属さないキーフレームＫＦｂをメモリ１０１上からアンロードする。

このようにすることで、フォークリフト１の特定の走行ルートに応じた、自己位置推定処理に必要な最小限のキーフレームのみがメモリ１０１上にロードされるので、自己位置推定処理の精度を維持しながらもメモリ１０１の使用領域を節約することができる。

なお、特定の走行ルートは、フォークリフト１の目的地やタスクの種類に応じて定められる態様であってもよい。例えば、生産した製品の入った箱をパレットの上に設置する生産ラインの周辺では、（１）空のパレットを生産ラインに搬送、（２）製品を搭載したパレットを生産ラインから製品保管倉庫に運搬して仮置き、とのタスクがある。地図読出部１００３は、（１）のタスクを開始する際に、パレット置き場から生産ラインまでの走行ルートに対応するキーフレームのみをロードする。そして、（２）のタスクに移行した場合には、地図読出部１００３は、（１）でロードしたキーフレームをアンロードするとともに、新たに、生産ラインから倉庫までの走行ルートに対応するキーフレームをロードする。

＜第３の実施形態＞
以下、第３の実施形態に係る自己位置推定装置、及び、これを備える移動体について、図１１、図１２を参照しながら説明する。

（リローカリゼーション処理部の処理）
図１１は、第３の実施形態に係るリローカリゼーション処理部の処理フローを示す図である。
図１２は、第３の実施形態に係るリローカリゼーション処理部の処理を説明する図である。

第３の実施形態に係るリローカリゼーション処理部１００４は、フォークリフト１のハードウェアの制約条件に基づいてキーフレームを探索する範囲（以下、単に探索範囲とも記載する。）を特定するとともに、当該探索範囲に含まれるキーフレームを探索する。
図１１に示す処理フローは、地図作成時又は実運用時において、自己位置推定処理の失敗（ロスト）が発生した際に、第３の実施形態に係るリローカリゼーション処理部１００４が実行するリローカリゼーションの処理の流れを示す。

図１１に示すように、自己位置推定装置１０のリローカリゼーション処理部１００４は、最初に、キーフレームの探索範囲をクリアする（ステップＳ３０）。
次に、リローカリゼーション処理部１００４は、キーフレームテーブル（図３）に登録された全てのキーフレームのうちいずれか１つを選択する（ステップＳ３１）。
次に、リローカリゼーション処理部１００４は、ステップＳ３１で選択したキーフレームが、ロスト発生直前に推定されたフォークリフトの位置を基準にして、フォークリフト１の最大速度の制約を満たす範囲に存在するか否かを判定する（ステップＳ３２）。

選択したキーフレームが、フォークリフト１の最大速度の制約を満たす範囲に存在する場合（ステップＳ３２；ＹＥＳ）、次に、リローカリゼーション処理部１００４は、選択したキーフレームが、ロスト発生直前に推定されたフォークリフトの姿勢を基準にして、フォークリフト１の最大角速度の制約を満たす範囲に存在するか否かを判定する（ステップＳ３３）。

選択したキーフレームが、フォークリフト１の最大角速度の制約を満たす範囲に存在する場合（ステップＳ３３；ＹＥＳ）、次に、リローカリゼーション処理部１００４は、選択したキーフレームの位置が、オドメトリによる推定位置と十分近いか否かを判定する（ステップＳ３４）。ここで、オドメトリによる推定位置とは、フォークリフト１に搭載されるタイヤ回転角センサ１２（図１）及び舵角センサ１３（図１）による検出結果を用いて推定されるフォークリフト１の位置である。リローカリゼーション処理部１００４は、例えば、ロスト発生直前にＯＲＢ−ＳＬＡＭで推定されたフォークリフトの位置を基準に、タイヤ回転角センサ１２（図１）及び舵角センサ１３（図１）による検出結果に基づく自己位置を推定する。

選択したキーフレームの位置が、オドメトリによる推定位置と十分近い場合（ステップＳ３４；ＹＥＳ）、次に、リローカリゼーション処理部１００４は、選択したキーフレームの姿勢が、ジャイロセンサ１４（図１）による推定姿勢と十分近いか否かを判定する（ステップＳ３５）。ジャイロセンサ１４による推定姿勢とは、例えば、ジャイロセンサ１４によって検出される角速度を積分することで求めたフォークリフト１の推定姿勢である。

選択したキーフレームの位置が、ジャイロセンサ１４による推定位置と十分近い場合（ステップＳ３４；ＹＥＳ）、リローカリゼーション処理部１００４は、選択したキーフレームを探索範囲に追加する（ステップＳ３６）。

他方、上記ステップＳ３２〜ステップＳ３５のうちのいずれか１つでもＮＯであった場合、リローカリゼーション処理部１００４は、選択したキーフレームを探索範囲に追加することなく次のステップに進む。

リローカリゼーション処理部１００４は、キーフレームテーブルに登録されている全てのキーフレームについて選択し、ステップＳ３２〜ステップＳ３６の処理を行ったか否かを判定する（ステップＳ３７）。
全てのキーフレームについて選択されていない場合（ステップＳ３７；ＮＯ）、リローカリゼーション処理部１００４は、次のキーフレームを選択し（ステップＳ３８）、当該キーフレームについてステップＳ３２〜ステップＳ３６の処理を行う。

全てのキーフレームについて選択された場合（ステップＳ３７；ＹＥＳ）、リローカリゼーション処理部１００４は、探索範囲に追加されているキーフレームのみを対象にリローカリゼーションを行う（ステップＳ３９）。

図１２に示す探索範囲ＴＲは、上記ステップＳ３２〜ステップＳ３８の判定処理を全て満たす領域である。即ち、リローカリゼーション処理部１００４は、上記ステップＳ３２〜ステップＳ３８の判定処理を行うことで、ロスト発生直前にＯＲＢ−ＳＬＡＭで推定された位置Ｐｃ及び姿勢Ａｃを基準に、ハードウェア上の制約によって規定される探索範囲ＴＲに属するキーフレームＫＦｄをリローカリゼーションにおける探索対象とし、探索範囲ＴＲに属さないキーフレームＫＦｅを探索対象としない。

以上の通り、第３の実施形態に係るリローカリゼーション処理部１００４は、自己位置推定処理で最後に推定された位置と、フォークリフト１のハードウェア上の制約の一つである最大速度とを用いて、当該フォークリフト１の現在の位置としてとり得る範囲を、キーフレームの探索範囲として特定する。

また、第３の実施形態に係るリローカリゼーション処理部１００４は、自己位置推定処理で最後に推定された姿勢と、フォークリフト１のハードウェア上の制約の一つである最大角速度とを用いて、当該フォークリフトの現在の姿勢としてとりうる範囲を、キーフレームの探索範囲として特定する。

また、第３の実施形態に係るリローカリゼーション処理部１００４は、オドメトリ（タイヤ回転角センサ１２及び舵角センサ１３）による推定位置から十分近い範囲（即ち、オドメトリによる推定位置の誤差とみなせる範囲）を、キーフレームの探索範囲として特定する。

また、第３の実施形態に係るリローカリゼーション処理部１００４は、ジャイロセンサ１４による推定姿勢から十分近い範囲（即ち、ジャイロセンサ１４による推定姿勢の誤差とみなせる範囲）を、キーフレームの探索範囲として特定する。

このようにすることで、フォークリフト１のハードウェアの制約上、明らかに外れている位置及び姿勢のキーフレームをリローカリゼーションの探索対象から外すことができるので、リローカリゼーションの処理の効率化、高速化を図ることができる。

なお、上述したステップＳ３２〜ステップＳ３５の判定処理の順番は適宜入れ替え可能であり、他の実施形態においては図１１に示す順番に限定されない。また、他の実施形態においては、ステップＳ３２〜ステップＳ３５の全てを行う必要はなく、ステップＳ３２〜ステップＳ３５の少なくともいずれか１つを実施するだけであっても上記効果を奏することができる。

＜第４の実施形態＞
以下、第４の実施形態に係る自己位置推定装置、及び、これを備える移動体について、図１３を参照しながら説明する。

（自己位置推定処理部の処理）
図１３は、第４の実施形態に係る自己位置推定処理部の機能を説明する図である。
第４の実施形態に係る自己位置推定処理部１００１は、フォークリフト１の位置及び姿勢の軌跡が平滑化されるように、自己位置推定処理によって推定される位置及び姿勢を修正する。

ここで、図７において、自己位置推定処理部１００１は、ＯＲＢ−ＳＬＡＭによる自己位置推定処理の結果（位置Ｐａ’、姿勢Ａａ’）に対して、カレントキーフレーム（キーフレームＫＦｎ）に紐づく変換行列Ｔｎを乗じることで、より精度の高い位置Ｐａ、姿勢Ａａを演算することを説明した。

この場合、以下のような課題が生じ得る。即ち、キーフレームＫＦｎに紐づく変換行列Ｔｎと、キーフレームＫＦｎ＋１に紐づく変換行列Ｔｎ＋１は、互いに異なる変換行列である。フォークリフト１の移動中において、自己位置推定処理に用いるカレントキーフレームは、ある位置を境に、例えば、キーフレームＫＦｎからキーフレームＫＦｎ＋１に切り替わる。そうすると、カレントキーフレームの切り替わりに伴い、真値とのずれの補正に用いられる変換行列が変換行列Ｔｎから変換行列Ｔｎ＋１に切り替わる。この場合、例えば、図１３に示すように、変換行列Ｔｎによって補正された位置及び姿勢と、変換行列Ｔｎ＋１によって補正された位置及び姿勢との間に“飛び”が発生し、非連続な軌跡となり得る。
そこで、本実施形態に係る自己位置推定処理部１００１は、自己位置推定処理によって推定された位置及び姿勢に対し、所定の平滑化処理を行う。自己位置推定処理部１００１は、この平滑化処理を施して得られた位置及び姿勢を、フォークリフト１の位置及び姿勢として出力する。この平滑化処理は、例えば、移動平均フィルタ等を用いて実装することができる。

このようにすることで、推定された自己位置が連続的な軌跡を描くようになる。これにより、位置及び姿勢の推定結果に基づいて自動操舵を行うフォークリフト１において、舵角の急激な変化を防止することができる。

（各実施形態の変形例）
以上、第１〜第４の実施形態に係る自己位置推定装置１０及びこれを備える移動体（フォークリフト１）について詳細に説明したが、自己位置推定装置１０及び移動体の具体的な態様は、上述のものに限定されることはなく、要旨を逸脱しない範囲内において種々の設計変更等を加えることは可能である。

例えば、自己位置推定装置１０を適用可能な移動体はフォークリフトに限定されず、無人航空機（ドローン）などにも適用可能である。この場合、自己位置推定装置１０は、無人航空機に搭載された加速度センサやジャイロセンサからの検出値を入力し、当該検出値が予め規定された条件を満たした場合に、現在の視界画像をキーフレームとして登録するものであってもよい。

上述の各実施形態においては、上述した自己位置推定装置１０の各種処理の過程は、プログラムの形式でコンピュータ読み取り可能な記録媒体に記憶されており、このプログラムをコンピュータが読み出して実行することによって上記各種処理が行われる。また、コンピュータ読み取り可能な記録媒体とは、磁気ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、半導体メモリ等をいう。また、このコンピュータプログラムを通信回線によってコンピュータに配信し、この配信を受けたコンピュータが当該プログラムを実行するようにしてもよい。

上記プログラムは、上述した機能の一部を実現するためのものであってもよい。更に、上述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であってもよい。

以上の通り、本発明に係るいくつかの実施形態を説明したが、これら全ての実施形態は、例として提示したものであり、発明の範囲を限定することを意図していない。これらの実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で種々の省略、置き換え、変更を行うことができる。これらの実施形態及びその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれる。

１ フォークリフト（移動体）
１０ 自己位置推定装置
１００ ＣＰＵ
１０００ 視界画像取得部（取得部）
１００１ 自己位置推定処理部（推定部）
１００２ キーフレーム登録部（登録部）
１００３ 地図読出部（読出部）
１００４ リローカリゼーション処理部（処理部）
１０１ メモリ
１０２ ストレージ
１０３ 通信インタフェース
１１ カメラ
１２ タイヤ回転角センサ
１３ 舵角センサ
１４ ジャイロセンサ
Ｍ 地図

Claims (17)

1. 移動体に搭載されたカメラから視界画像を取得する取得部と、
   前記視界画像上における特徴点の位置と、世界座標上における複数の特徴点の位置が記録されてなる地図とに基づいて、前記世界座標上における前記移動体の位置及び姿勢を推定する自己位置推定処理を行う推定部と、
   前記視界画像を、当該視界画像上に含まれる新たな特徴点を前記地図に追加するためのキーフレームとして登録する登録部と、
   を備え、
   前記登録部は、前記移動体の移動が、当該移動体の位置及び姿勢の少なくともいずれかについて予め規定された条件を満たした場合に、前記視界画像を前記キーフレームとして登録する
   自己位置推定装置。
2. 前記登録部は、前記移動体の位置と、当該位置に紐づけられる１つの前記キーフレームとの距離が所定の距離閾値以上離れている場合に、前記視界画像をキーフレームとして登録する
   請求項１に記載の自己位置推定装置。
3. 前記登録部は、移動中の前記移動体の角速度が所定の角速度閾値以上となった場合に、前記視界画像をキーフレームとして登録する
   請求項１または請求項２に記載の自己位置推定装置。
4. 前記登録部は、消去防止フラグの有無に基づいて、登録済の複数の前記キーフレームに対する間引き処理を行い、
   複数の前記キーフレームの１つである対象キーフレームに対し、当該対象キーフレームよりも１つ前に登録されたキーフレームと、当該対象キーフレームよりも１つ後に登録されたキーフレームとの位置及び姿勢の少なくともいずれかの関係に基づいて、当該対象キーフレームに前記消去防止フラグを付するか否かを判定する
   請求項１から請求項３のいずれか一項に記載の自己位置推定装置。
5. 前記地図のうちの一部の領域をメモリ上に読み出す読出部を更に備える
   請求項１から請求項４のいずれか一項に記載の自己位置推定装置。
6. 前記読出部は、前記地図のうち、前記移動体の位置を基準とする前記一部の領域をメモリ上に読み出す
   請求項５に記載の自己位置推定装置。
7. 前記読出部は、前記地図のうち、前記移動体の幅方向と比較して前記移動体の前後方向に広い前記一部の領域をメモリ上に読み出す
   請求項６に記載の自己位置推定装置。
8. 前記読出部は、前記地図のうち、特定の走行ルートに対応する前記一部の領域をメモリ上に読み出す
   請求項５から請求項７のいずれか一項に記載の自己位置推定装置。
9. 前記自己位置推定処理に失敗した場合に前記キーフレームを探索するリローカリゼーションを行う処理部を備え、
   前記処理部は、前記移動体のハードウェアの制約条件に基づいて探索範囲を特定するとともに、当該探索範囲に含まれるキーフレームを探索する
   請求項１から請求項８のいずれか一項に記載の自己位置推定装置。
10. 前記処理部は、前記自己位置推定処理で推定された位置と、前記移動体の最大速度とに基づいて、当該移動体の現在の位置としてとりうる範囲を前記探索範囲として特定する
    請求項８に記載の自己位置推定装置。
11. 前記処理部は、前記自己位置推定処理で推定された姿勢と、前記移動体の最大角速度とに基づいて、当該移動体の現在の姿勢としてとり得る範囲を前記探索範囲として特定する
    請求項８又は請求項９に記載の自己位置推定装置。
12. 前記処理部は、前記自己位置推定処理で推定された位置を基準とする、オドメトリによる推定位置の誤差とみなせる範囲を前記探索範囲として特定する
    請求項８から請求項１０のいずれか一項に記載の自己位置推定装置。
13. 前記処理部は、前記自己位置推定処理で推定された姿勢を基準とする、ジャイロセンサによる推定姿勢の誤差とみなせる範囲を前記探索範囲として特定する
    請求項８から請求項１１のいずれか一項に記載の自己位置推定装置。
14. 前記推定部は、前記世界座標上における前記移動体の位置及び姿勢の軌跡が平滑化されるように、前記自己位置推定処理によって推定された位置及び姿勢を修正する
    請求項１から請求項１３のいずれか一項に記載の自己位置推定装置。
15. 前記推定部は、移動平均フィルタを用いて、前記自己位置推定処理によって推定された位置及び姿勢を修正する
    請求項１４に記載の自己位置推定装置。
16. 移動体に搭載されたカメラから視界画像を取得するステップと、
    前記視界画像上における特徴点の位置と、世界座標上における複数の特徴点の位置が記録されてなる地図とに基づいて、前記世界座標上における前記移動体の位置及び姿勢を推定する自己位置推定処理を行うステップと、
    前記視界画像を、当該視界画像上に含まれる新たな特徴点を前記地図に追加するためのキーフレームとして登録するステップと、
    を有し、
    前記キーフレームとして登録するステップでは、前記移動体の移動が、当該移動体の位置及び姿勢の少なくともいずれかについて規定された条件を満たした場合に、前記視界画像を前記キーフレームとして登録する
    自己位置推定方法。
17. 自己位置推定装置のコンピュータに、
    移動体に搭載されたカメラから視界画像を取得するステップと、
    前記視界画像上における特徴点の位置と、世界座標上における複数の特徴点の位置が記録されてなる地図とに基づいて、前記世界座標上における前記移動体の位置及び姿勢を推定する自己位置推定処理を行うステップと、
    前記視界画像を、当該視界画像上に含まれる新たな特徴点を前記地図に追加するためのキーフレームとして登録するステップと、
    を実行させ、
    前記キーフレームとして登録するステップでは、前記移動体の移動が、当該移動体の位置及び姿勢の少なくともいずれかについて規定された条件を満たした場合に、前記視界画像を前記キーフレームとして登録する
    プログラム。

Images