JP7482453B2 - 測位装置及び移動体 - Google Patents

Description

本開示は、車両などの移動体の位置を測定する測位装置と、そのような測位装置を備えた移動体とに関する。

荷物運搬用の車両を用いて予め決められた領域内又は予め決められた地点間で荷物を移動させるとき、車両の位置を測定して追跡することが求められる場合がある。例えば、特許文献１は、ＧＰＳ、無線ＬＡＮ測位、赤外線測位などの測位技術を用いて車両の位置を測位する荷物所在管理装置を開示している。

屋外で移動する車両を測位する場合、一般的には、ＧＰＳを用いた測位方法が使用される。一方、倉庫又は工場などの屋内では、ＧＰＳ衛星からの電波を受信できないので、ＧＰＳを用いた測位方法は使用できない。屋内の測位方法として、例えば、ＵＷＢ（ｕｌｔｒａ ｗｉｄｅ ｂａｎｄ）、Ｗｉ－Ｆｉ、又はＢＬＥ（Ｂｌｕｅｔｏｏｔｈ（登録商標） Ｌｏｗ Ｅｎｅｒｇｙ）などの無線信号を用いたものがある。しかしながら、無線信号を用いた測位方法は、無線信号を送信するための多数の無線送信機を車両の移動範囲に設置する必要があるので、初期導入コストが高い。また、屋内の測位方法として、ＰＤＲ（Ｐｅｄｅｓｔｒｉａｎ Ｄｅａｄ Ｒｅｃｋｏｎｉｎｇ：歩行者自律航法）と呼ばれるものがある。しかしながら、ＰＤＲは位置を高精度に測定することが困難である。

無線信号を送信するための多数の無線送信機を必要とすることなく、車両などの移動体の位置を高精度に測定して追跡するために、例えば、非特許文献１に開示されるように、Visual-SLAM（Visual Simultaneous Localization and Mapping）と呼ばれる技術がある。Visual-SLAMによれば、撮影装置を備えた移動体がその周囲を撮影しながら移動し、撮影された複数の画像における特徴点の移動量に基づいて移動体の移動量が計算される。これにより、移動体の現在の位置を推定したり、移動体の軌跡に基づいて地図を生成したりすることができる。

特開２０１１－２１９２２９号公報

R. Mur-Artal, et al., "ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras", IEEE Transactions on Robotics, Volume: 33, Issue: 5, Oct. 2017

工場及び倉庫等など、日々背景が変化するシーンにおいては、事前に作成した地図を用いて現在の位置を特定することは困難である。この場合、Visual-SLAMによって得られる移動体の位置は、ある基準位置（例えば、移動体が移動を開始した位置）に対する相対位置として計算されるので、時間の経過に従って誤差が累積的に増大する。従って、撮影装置を用いて、従来よりも小さな誤差で移動体の位置を測定することができる測位装置が求められる。

本開示の目的は、撮影装置を用いて移動体の位置を測定する測位装置であって、従来よりも小さな誤差で移動体の位置を測定することができる測位装置を提供することにある。

本開示の一態様によれば、
移動体に搭載された撮影装置によって撮影された複数の画像に基づいて、所定の基準位置及び基準姿勢に対する前記移動体の相対位置及び相対姿勢を示す前記移動体の第１の位置及び第１の姿勢を計算する第１の計算器と、
予め決められた位置に配置されて視覚的に識別可能な複数のマーカーの識別子、位置、及び姿勢の情報と、前記移動体のための通路を含むマップの情報とを格納する記憶装置と、
前記撮影装置によって撮影された画像から前記複数のマーカーのうちの１つを抽出し、抽出された前記１つのマーカーの位置及び姿勢に基づいて、前記マップにおける前記移動体の位置及び姿勢を示す前記移動体の第２の位置及び第２の姿勢を計算し、前記１つのマーカーを原点とするマーカー座標系における前記移動体の位置及び姿勢に基づいて、前記１つのマーカーの面の法線に対する前記移動体の方向への角度を計算する第２の計算器と、
前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正し、前記移動体の補正された位置及び補正された姿勢を生成する補正器とを備え、
前記補正器は、前記角度が第１の閾値以下である前記マーカーの位置及び姿勢に基づいて計算される前記第２の位置及び前記第２の姿勢を使用せず、前記角度が前記第１の閾値より大きい前記マーカーの位置及び姿勢に基づいて計算される前記第２の位置及び前記第２の姿勢を使用して、前記補正された位置及び前記補正された姿勢を生成する。

これらの概括的かつ特定の態様は、システム、方法、コンピュータプログラム並びにシステム、方法及びコンピュータプログラムの任意の組み合わせにより実現してもよい。

本開示の一態様によれば、第２の位置及び第２の姿勢に基づいて第１の位置及び第１の姿勢を補正することにより、移動体の位置及び姿勢を高精度に測定することができる。また、本開示の一態様によれば、認識されたマーカーを信頼できるか否かを判断し、マーカーを信頼できるときのみ、第２の位置及び第２の姿勢に基づいて移動体の位置及び姿勢を補正することにより、移動体の位置及び姿勢をより高精度に測定することができる。

第１の実施形態に係る車両１の構成を示す概略図である。 図１の車両１を含む測位システムの構成を示すブロック図である。 図２の測位装置１２の構成を示すブロック図である。 図１の車両１が移動する通路１０１を含む倉庫１００のマップである。 図４のマーカー４の例を示す図である。 図３の記憶装置３５に格納されたマーカー情報の例を示すテーブルである。 図１の撮影装置１１によって撮影された画像４０の例を示す図である。 図２の測位装置１２によって実行される測位処理を示すフローチャートである。 図８のステップＳ２（相対位置計算処理）のサブルーチンを示すフローチャートである。 図３の画像処理器３１によって抽出される特徴点であって、（ａ）は時刻ｎにおける画像４０（ｎ）から抽出される特徴点Ｆ１，Ｆ２を示し、（ｂ）は時刻ｎ’における画像４０（ｎ’）から抽出される特徴点Ｆ１’，Ｆ２’を示す図である。 図８のステップＳ３（絶対位置計算処理）のサブルーチンを示すフローチャートである。 マーカー座標系におけるマーカー４の頂点の座標を示す図である。 図１の撮影装置１１によって撮影された画像４０におけるマーカー４の頂点の座標を示す図である。 図８のステップＳ４（補正処理）のサブルーチンを示すフローチャートである。 図１４のステップＳ３１において計算される補正移動量を説明する図である。 図１４のステップＳ３３において計算される補正回転量を説明する図である。 図１１のステップＳ２４（マーカー評価処理）のサブルーチンを示すフローチャートである。 図１の撮影装置１１によって撮影された画像４０の例を示す図である。 図３の画像認識器３３によって検出されたマーカー４と、マーカー４であると誤検出されたオブジェクトとを含むテーブルである。 図１の撮影装置１１によって撮影され、車両１の絶対位置及び絶対姿勢の計算に適したマーカー４を含む画像４０の例を示す図である。 図１の撮影装置１１によって撮影され、車両１の絶対位置及び絶対姿勢の計算に適さないマーカー４を含む画像４０の例を示す図である。 マーカー座標系における撮影装置１１の位置の反転を説明する図である。 図３の位置計算器３４によって計算されたマーカー座標系における撮影装置１１の位置の変化を示すテーブルである。 図１の撮影装置１１によって撮影され、車両１の絶対位置及び絶対姿勢の計算に適した幅を有するマーカー４の像の例を示す図である。 図１の撮影装置１１によって撮影され、車両１の絶対位置及び絶対姿勢の計算に適した幅をもたないマーカー４の像の例を示す図である。 図１７のステップＳ４３における閾値を変化させたときの車両１の絶対位置の誤差を示すグラフである。 マーカー座標系において、車両１の絶対位置及び絶対姿勢の計算に適したマーカー４の像を撮影可能な角度及び距離を説明する図である。 車両１の絶対位置及び絶対姿勢の計算に適したマーカー４の像を撮影するときの角度の条件を説明する図である。 図２８の撮影装置１１によって撮影されたマーカー４を含む画像の例を示す図である。 第１の実施形態の比較例に係る補正処理を実行することによって計算された車両１の軌跡１０３を示す図である。 図１７のマーカー評価処理を実行することによって計算された車両１の軌跡１０４を示す図である。 第２の実施形態に係る測位装置１２Ｂの構成を示すブロック図である。

以下、本開示に係る実施形態について図面を参照して説明する。なお、以下の各実施形態において、同様の構成要素については同一の符号を付している。

［第１の実施形態］
図１～図２４を参照して、第１の実施形態に係る測位装置及びそれを備えた移動体について説明する。

［第１の実施形態の構成］
［全体構成］
図１は、第１の実施形態に係る車両１の構成を示す概略図である。車両１は、例えばフォークリフト又はトラックなど、有人の乗物であってもよい。また、車両１は、荷物３を搭載する荷台１ａを備える。車両１は、荷物３を荷台１ａに積み卸しするための昇降機構１ｂをさらに備えてもよい。また、車両１は、前進、後進、操舵、及び停止などのユーザ操作を受けるコンソール１ｃを備える。また、車両１の車体には、車両１に対して予め決められた向き（前方、後方、側方、上方、及び／又は下方）を撮影するように、撮影装置１１が設置されている。

図２は、図１の車両１を含む測位システムの構成を示すブロック図である。図２の測位システムは、少なくとも１つの車両１と、サーバ装置２とを含む。各車両１は、撮影装置１１によって撮影された画像に基づいて、その位置を測定する測位装置１２を備える。サーバ装置２は、各車両１からそれらの位置を取得し、各車両１の位置を記録する。

［車両１の構成］
車両１は、撮影装置１１、測位装置１２、通信装置１３、表示装置１４、及び駆動機構１５を備える。

撮影装置１１は、車両１の移動中に、所定時間ごとに、車両１に対して予め決められた向きにおける被写体の画像を生成する。撮影装置１１は、例えば、少なくとも１つのカメラを含む。撮影装置１１は、所定時間ごとに静止画像を撮影してもよく、動画像の一連のフレームから所定時間ごとにフレームを抽出してもよい。撮影装置１１は、撮影した画像を測位装置１２に送る。撮影装置１１は、各画像に、当該画像を撮影した時刻のタイムスタンプを付与する。

測位装置１２は、撮影装置１１によって撮影された画像に基づいて、車両１の位置を測定する。測位装置１２は、撮影装置１１によって撮影された複数の画像から特徴点を抽出し、抽出された特徴点を画像間で対応付けし、画像間の特徴点の変化量に基づいて、所定の基準位置及び基準姿勢に対する車両１の相対位置及び相対姿勢を計算する。また、測位装置１２は、撮影装置１１によって撮影された画像から、予め決められた位置に配置されて視覚的に識別可能な複数のマーカーのうちの１つを抽出し、抽出された１つのマーカーに基づいて、予め与えられたマップにおける車両１の絶対位置及び絶対姿勢を計算する。測位装置１２はさらに、絶対位置及び絶対姿勢に基づいて相対位置及び相対姿勢を補正する。

本明細書において、車両１の「姿勢」は、例えば、所定の座標系（後述する「世界座標系」又は「マーカー座標系」）の座標軸に対する車両１の進行方向の角度を示す。

通信装置１３は、Ｗｉ－Ｆｉ又はＢｌｕｅｔｏｏｔｈなどのモジュール及びその制御プログラムを備え、サーバ装置２と無線で通信する。通信装置１３は、測位装置１２によって計算された車両１の位置及び姿勢をサーバ装置２に送信する。

表示装置１４は、車両１の位置をマップの上に表示してもよい。また、表示装置１４は、車両１の動作に関する警報などを表示してもよい。

駆動機構１５は、車両１のエンジン又はモータ、操舵装置、制動装置、及びそれらの制御装置などを含む。駆動機構１５は、例えば、コンソール１ｃを介してユーザによって制御される。

［サーバ装置２の構成］
図２のサーバ装置２は、処理装置２１、通信装置２２、入力装置２３、記憶装置２４、及び表示装置２５を備える。処理装置２１は、例えば、プロセッサ及びメモリなどを含む汎用のコンピュータである。通信装置２２は、車両１の通信装置１３と通信可能に接続される。入力装置２３は、キーボード及びポインティングデバイスなどを含む。記憶装置２４は、車両１から受信した車両１の位置及び姿勢を記録する。表示装置２５は、車両１から受信した車両１の位置及び姿勢を表示する。処理装置２１は、通信装置２２を介して各車両１からそれらの位置を取得し、各車両１の位置を記憶装置２４に記録し、また、各車両１の位置を表示装置２５に表示する。

表示装置２５は、車両１の測位装置１２によって計算された車両１の位置及び姿勢を表示する。処理装置２１は、車両１の移動範囲（倉庫又は工場など）のマップを予め取得し、測位装置１２によって計算された車両１の位置及び姿勢をこのマップ上に重畳して表示装置２５に表示してもよい。それに代わって、処理装置２１は、車両１の移動経路に基づいて処理装置２１自体によりマップを生成し、このマップを表示装置２５に表示してもよい。

［測位装置１２の構成］
図３は、図２の測位装置１２の構成を示すブロック図である。測位装置１２は、画像処理器３１、相対位置計算器３２、画像認識器３３、絶対位置計算器３４、記憶装置３５、及び補正器３６を備える。

記憶装置３５は、予め決められた位置に配置されて視覚的に識別可能な複数のマーカー４の識別子、位置、及び姿勢の情報と、車両１のための通路を含むマップ（例えば、図４を参照して説明する倉庫１００のマップ）の情報とを格納する。マーカー４の位置は、所定の基準位置に対する相対位置として表されてもよく、及び／又は、マップに関連付けて表されてもよい。

図４は、図１の車両１が移動する通路１０１を含む倉庫１００のマップである。倉庫１００は、複数の通路１０１及び複数の棚１０２などの構造物を含む。倉庫１００における予め決められた複数の位置に、複数のマーカー４が予め配置される。図１の車両１は、通路１０１を移動して、ある棚１０２から他の棚１０２に荷物３を搬送する。車両１及び各マーカー４の位置は、倉庫１００の全体に対して決められた世界座標系（Ｘｗ，Ｙｗ，Ｚｗ）によって表される。

図５は、図４のマーカー４の例を示す図である。図５の例では、マーカー４は正方形の平板として形成される。マーカー４は、その１つの面において、マーカー４自体の識別子を符号化した視覚的に識別可能なパターンを有する。図５の例では、マーカー４は、縦横に７×７個の白又は黒の正方形のセルからなるパターンを有する。マーカー４のパターンはさらに、例えば拡張現実の分野で使用されるマーカー（「ＡＲマーカー」ともいう）のように、マーカー４を撮影した画像からマーカー４自体の姿勢を検出可能であるように形成される。各マーカー４は、その任意の点（例えば、中心、又は１つの頂点）を原点とするマーカー座標系（Ｘｍ，Ｙｍ，Ｚｍ）を有する。図５の下段及び他の図面では、Ｘｍ－Ｙｍ面に沿った面の中央における矢印によってマーカー４の正面（Ｚｍ軸の正の向き）を示す。

図６は、図３の記憶装置３５に格納されたマーカー情報の例を示すテーブルである。図６の例では、図４に示す２つのマーカー４の情報を示す。各マーカー４は、識別子００１、００２を有する。この識別子は、マーカー４のパターンに符号化されている。また、各マーカー４は、世界座標系（Ｘｗ，Ｙｗ，Ｚｗ）において所定の座標を有する。また、各マーカー４は、その正面（Ｚｍ軸の正の向き）がＸｗ－Ｙｗ面内においてＸｗ軸を基準として角度θ（すなわち方位角）を有するような姿勢で配置される。各マーカー４の姿勢は、方位角及び仰角によって表されてもよい。また、各マーカー４は、３０ｃｍ×３０ｃｍのサイズを有する。

記憶装置３５は、すべてのマーカー４について、例えば、図６に示すような項目を含むマーカー情報を格納する。また、記憶装置３５は、すべての通路１０１について、その向き、寸法、及び配置を含むマップ情報を格納する。

図７は、図１の撮影装置１１によって撮影された画像４０の例を示す図である。画像４０は複数の特徴点４１を含む。特徴点４１は、輝度値あるいは色が周囲の画素と区別でき、その位置を正確に決定することができる点である。特徴点４１は、例えば、車両１が移動する通路１０１又は棚１０２などの構造物の頂点又はエッジ、床、壁、又は天井の模様から検出される。また、車両１がマーカー４の近傍を通過するとき、画像４０はマーカー４を含む。画像４０における特徴点４１及びマーカー４の位置は、例えば、画像４０の任意の点（例えば、左上のコーナー）を原点とする画像座標系（Ｘｉ，Ｙｉ）によって表される。

再び図３を参照すると、画像処理器３１は、所定時間だけ離れた複数の時刻において撮影装置１１によってそれぞれ撮影された複数の画像から、対応する特徴点の座標を抽出する。相対位置計算器３２は、時間的に隣接した２つの画像における特徴点の移動量に基づいて、車両１の移動量を計算する。これにより、相対位置計算器３２は、複数の画像の特徴点の座標に基づいて、所定の基準位置及び基準姿勢（例えば、車両１が移動を開始したときの位置及び姿勢）に対する車両１の相対位置及び相対姿勢を計算する。相対位置計算器３２は、例えば、Visual-SLAM又はVisual-Odometryなど、公知の画像処理及び測位の技術を用いて、車両１の相対位置及び相対姿勢を計算してもよい。基準位置及び基準姿勢は、記憶装置３５に格納されたマップの情報に関連付けられる。また、相対位置計算器３２は、相対位置及び相対姿勢に、それらの計算に関連付けられた画像（時間的に隣接した２つの画像のうちの後者）のタイムスタンプを付与する。

相対位置計算器３２は、計算した車両１の位置を、例えば、直交座標（ＸＹＺ座標）により表してもよい。相対位置計算器３２は、計算した車両１の位置及び時刻に基づいて、車両１の速度及び／又は加速度を計算してもよい。相対位置計算器３２は、計算した車両１の姿勢を、ロール（左右の傾き）、ピッチ（前後の傾き）、及びヨー（床面に垂直な軸（すなわち、図４のＺｗ軸）の周りの回転）により表してもよい。これにより、地面に平行な水平面内における車両１の向きだけでなく、車両１の車体の傾斜を表したり、車両１の高さ方向の移動を表したりすることができる。

本明細書では、画像処理器３１及び相対位置計算器３２を合わせて、「第１の計算器」ともいう。また、本明細書では、相対位置及び相対姿勢を「第１の位置」及び「第１の姿勢」ともいう。

画像認識器３３は、撮影装置１１によって撮影された画像から、予め決められた位置に配置されて視覚的に識別可能な複数のマーカー４のうちの１つを抽出する。絶対位置計算器３４は、抽出された１つのマーカー４の位置及び姿勢に基づいて、記憶装置３５に格納されたマーカー４の情報及びマップの情報を参照することにより、マップ（すなわち世界座標系）における車両１の位置及び姿勢を示す車両１の絶対位置及び絶対姿勢を計算する。また、絶対位置計算器３４は、絶対位置及び絶対姿勢に、それらの計算に関連付けられた画像のタイムスタンプを付与する。

本明細書では、画像認識器３３及び絶対位置計算器３４を合わせて、「第２の計算器」ともいう。また、本明細書では、絶対位置及び絶対姿勢を「第２の位置」及び「第２の姿勢」ともいう。

補正器３６は、絶対位置及び絶対姿勢に基づいて相対位置及び相対姿勢を補正し、車両１の補正された位置及び補正された姿勢を生成する。補正器３６は、相対位置及び相対姿勢のタイムスタンプと、絶対位置及び絶対姿勢のタイムスタンプとに基づいて、絶対位置及び絶対姿勢を相対位置及び相対姿勢と同期させる。補正器３６は、例えば、予め決められた閾値より小さい時間差を有し、最も近接したタイムスタンプを有する相対位置及び相対姿勢と絶対位置及び絶対姿勢とを、同じ画像から計算されたものとみなしてもよい。

測位装置１２の各構成要素３１～３６のうちの少なくとも一部が一体化されてもよい。例えば、画像処理器３１及び画像認識器３３を一体化してもよい。また、測位装置１２の各構成要素３１～３６は、専用回路として実装されてもよく、汎用のプロセッサによって実行されるプログラムとして実装されてもよい。

［第１の実施形態の動作］
補正器３６は、前述のように、Visual-SLAMなどを用いて特徴点４１に基づいて計算された相対位置及び相対姿勢を、マーカー４に基づいて計算された絶対位置及び絶対姿勢に基づいて補正する。この補正処理は、車両１の絶対位置及び絶対姿勢が正しいことを前提とし、従って、マーカー４を撮影した画像からマーカー４の姿勢が正しく認識されていることを前提としている。しかしながら、マーカー４ではないオブジェクトがマーカー４として誤検出されることがある。また、マーカー４の姿勢を正しく認識するためには、マーカー４を適切な角度及び適切な距離から撮影する必要がある。マーカー４を撮影した画像から認識されたマーカー４自体の位置及び姿勢に誤差があると、車両１の絶対位置及び絶対姿勢にも誤差が生じ、測位装置１２によって計算される車両１の位置及び／又は姿勢の精度が低下するおそれがある。

このため、第１の実施形態に係る測位装置１２では、絶対位置計算器３４は、マーカー４を適切な角度及び適切な距離から撮影しているか否か、すなわち、認識されたマーカー４を信頼できるか否かを判断し、マーカー４を信頼できるときのみ、マーカー４の位置及び姿勢に基づいて計算される車両１の絶対位置及び絶対姿勢を補正器３６に送る。その結果、補正器３６は、マーカー４を信頼できるときのみ、絶対位置及び絶対姿勢に基づいて車両１の位置及び姿勢を補正する。これにより、正しく認識されたマーカー４の位置及び姿勢に基づいて、車両１の位置及び姿勢を正しく補正し、従って、車両１の位置及び姿勢を高精度に測定することができる。

以下、測位装置１２の動作について詳細に説明する。

［測位処理の全体］
図８は、図２の測位装置１２によって実行される測位処理を示すフローチャートである。

ステップＳ１において、測位装置１２は、撮影装置１１によって撮影された画像を取得する。ステップＳ２において、画像処理器３１及び相対位置計算器３２は、相対位置計算処理を実行し、車両１の相対位置及び相対姿勢を計算する。ステップＳ３において、画像認識器３３及び絶対位置計算器３４は、絶対位置計算処理を実行し、車両１の絶対位置及び絶対姿勢を計算する。ステップＳ２及びＳ３は、図８に示すように並列に実行されてもよく、逐次に実行されてもよい。ステップＳ４において、補正器３６は、補正処理を実行し、絶対位置及び絶対姿勢に基づいて相対位置及び相対姿勢を補正し、車両１の補正された位置及び補正された姿勢を生成する。ステップＳ５において、補正器３６は、車両１の補正された位置及び補正された姿勢を、通信装置１３及び表示装置１４に出力する。

［相対位置計算処理］
図９は、図８のステップＳ２（相対位置計算処理）のサブルーチンを示すフローチャートである。

ステップＳ１１において、画像処理器３１は、所定時間だけ離れた第１及び第２の時刻にそれぞれ撮影された（例えば、時間的に隣接したフレームの）第１及び第２の画像を取得する。

ステップＳ１２において、画像処理器３１は、第１の画像から特徴点を検出する。画像から特徴点を検出するために、例えば、ＦＡＳＴ（Ｆｅａｔｕｒｅｓ ｆｒｏｍ Ａｃｃｅｌｅｒａｔｅｄ Ｓｅｇｍｅｎｔ Ｔｅｓｔ）のような画像処理技術を使用してもよい。

ステップＳ１３において、画像処理器３１は、第２の画像から、第１の画像の特徴点に対応する特徴点を検出する。画像間で対応する特徴点を検出するために、例えば、ＫＬＴ（Ｋａｎａｄｅ－Ｌｕｃａｓ－Ｔｏｍａｓｉ）トラッカーのような公知の画像処理技術を使用してもよい。

図１０は、図３の画像処理器３１によって抽出される特徴点を示す図である。図１０（ａ）は時刻ｎにおける画像４０（ｎ）から抽出される特徴点Ｆ１，Ｆ２を示す。図１０（ｂ）は時刻ｎ’における画像４０（ｎ’）から抽出される特徴点Ｆ１’，Ｆ２’を示す。図１０（ａ）の画像４０（ｎ）の画像座標系において、特徴点Ｆ１は座標（ｘｉ１，ｙｉ１）を有し、特徴点Ｆ２は座標（ｘｉ２，ｙｉ２）を有する。図１０（ｂ）の画像４０（ｎ’）の画像座標系において、特徴点Ｆ１’は座標（ｘｉ１’，ｙｉ１’）を有し、特徴点Ｆ２’は座標（ｘｉ２’，ｙｉ２’）を有する。図１０（ｂ）の特徴点Ｆ１’，Ｆ２’は、図１０（ａ）の特徴点Ｆ１，Ｆ２にそれぞれ対応する。

図９のステップＳ１４において、画像処理器３１は、第１及び第２の画像における対応する特徴点の座標の組を取得する。画像処理器３１は、例えば、特徴点Ｆ１，Ｆ１’の座標の組（ｘｉ１，ｙｉ１，ｘｉ１’，ｙｉ１’）を取得し、特徴点Ｆ２，Ｆ２’の座標の組（ｘｉ２，ｙｉ２，ｘｉ２’，ｙｉ２’）を取得する。

ステップＳ１５において、相対位置計算器３２は、ステップＳ１４において取得された特徴点の座標に基づいて、例えば５点アルゴリズムを用いて、３×３個の要素からなる基礎行列Ｅを計算する。

ステップＳ１６において、相対位置計算器３２は、基礎行列Ｅに対して特異値分解を行うことにより、第１及び第２の画像を撮影した時刻の間における車両１の移動を表す回転行列Ｒ及び並進ベクトルｔを計算する。回転行列Ｒは、第１及び第２の画像を撮影した時刻の間における車両１の姿勢の変化を示す。並進ベクトルｔは、第１及び第２の画像を撮影した時刻の間における車両１の位置の変化を示す。

回転行列Ｒ及び並進ベクトルｔの計算は、例えば、以下のように定式化される。

基礎行列Ｅは、特異値分解を行うことにより、Ｅ＝ＵΣＶ^Ｔとして表される。ここで、Σは、３×３個の要素からなる対角行列Σであり、Ｕ，Ｖは、３×３個の要素からなる直交行列である。

回転行列Ｒは、３×３個の要素からなる次式の行列Ｗを用いて、Ｒ＝ＵＷ^－１Ｖ^Ｔにより計算される。

また、並進ベクトルｔを計算するために、３×３個の要素からなる行列Ｔ＝ＶＷΣＶ^Ｔが計算される。行列Ｔは、Ｅ＝ＴＲを満たし、次式で表される。

並進ベクトルｔは、行列Ｔの成分により、ｔ＝（ｔ_ｘ，ｔ_ｙ，ｔ_ｚ）^Ｔとして表される。

ステップＳ１７において、相対位置計算器３２は、車両１の相対位置及び相対姿勢を計算して出力する。直前の時刻ｎ－１において車両１が相対位置ｔ（ｎ－１）及び相対姿勢Ｒ（ｎ－１）を有するとき、現在の時刻ｎにおいて、車両１の相対位置ｔ（ｎ）は、ステップＳ１６において計算された並進ベクトルｔを用いて、ｔ（ｎ）＝ｔ（ｎ－１）＋ｔＲ（ｎ－１）により表される。また、現在の時刻ｎにおいて、車両１の相対姿勢Ｒ（ｎ）は、ステップＳ１６において計算された回転行列Ｒを用いて、Ｒ（ｎ）＝ＲＲ（ｎ－１）で表される。このように、相対位置計算器３２は、複数の並進ベクトルを累積的に加算し、複数の回転行列を累積的に乗算することにより、所定の基準位置及び基準姿勢に対する車両１の相対位置及び相対姿勢を計算する。

［絶対位置計算処理］
図１１は、図８のステップＳ３（絶対位置計算処理）のサブルーチンを示すフローチャートである。

ステップＳ２１において、画像認識器３３は、画像からマーカー４を検出する。ここで、画像認識器３３は、画像座標系において四角形のマーカー４の４つの頂点（コーナー）の座標を検出し、また、マーカー４のパターンを復号してマーカー４の識別子を取得する。なお、画像認識器３３は、マーカー４の４つの頂点に代えて、予め決められたいくつかの点の座標を検出してもよい。

図１２は、マーカー座標系におけるマーカー４の頂点の座標を示す図である。マーカー座標系（Ｘｍ，Ｙｍ，Ｚｍ）において、マーカー４の４つの頂点は、座標（ｘｍ０，ｙｍ０，ｚｍ０）、（ｘｍ１，ｙｍ１，ｚｍ１）、（ｘｍ２，ｙｍ２，ｚｍ２）、及び（ｘｍ３，ｙｍ３，ｚｍ３）をそれぞれ有する。マーカー４の寸法は既知であるので、マーカー座標系におけるマーカー４の４つの頂点の座標もまた既知である。例えば、図１２のマーカー４の左上の頂点がマーカー座標系（Ｘｍ，Ｙｍ，Ｚｍ）の原点であり、かつ、マーカー４が３０ｃｍ×３０ｃｍのサイズを有する場合、マーカー４の頂点は、例えば、座標（ｘｍ０，ｙｍ０，ｚｍ０）＝（０，０，０）、（ｘｍ１，ｙｍ１，ｚｍ１）＝（０．３，０，０）、（ｘｍ２，ｙｍ２，ｚｍ２）＝（０，０．３，０）、及び（ｘｍ３，ｙｍ３，ｚｍ３）＝（０．３，０．３，０）を有する。

図１３は、図１の撮影装置１１によって撮影された画像４０におけるマーカー４の頂点の座標を示す図である。画像座標系（Ｘｉ，Ｙｉ）において、マーカー４の４つの頂点は、座標（ｘｉ０，ｙｉ０）、（ｘｉ１，ｙｉ１）、（ｘｉ２，ｙｉ２）、及び（ｘｉ３，ｙｉ３）をそれぞれ有する。

図１１のステップＳ２２において、絶対位置計算器３４は、ステップＳ２１において検出されたマーカー４の座標に基づいて、撮影装置１１を原点とする３次元座標系（カメラ座標系）におけるマーカー４の位置及び姿勢（すなわち、撮影装置１１から見たマーカー４の位置及び姿勢）を計算する。例えば、絶対位置計算器３４は、２次元の画像座標系におけるマーカー４の４つの頂点の座標と、３次元のマーカー座標系におけるマーカー４の４つの頂点の座標とに基づいて、ＰｎＰ（perspective n point）問題を解くことにより、撮影装置１１から見たマーカー４の位置及び姿勢を計算する。

ステップＳ２３において、絶対位置計算器３４は、マーカー座標系における撮影装置１１の位置及び姿勢（すなわち、マーカー４から見た撮影装置１１の位置及び姿勢）を計算する。ここで、撮影装置１１から見たマーカー４の位置を並進ベクトルｔにより示し、撮影装置１１から見たマーカー４の姿勢を回転行列Ｒにより示す。この場合、マーカー４から見た撮影装置１１の姿勢は、Ｒ^－１により表され、マーカー４から見た撮影装置１１の位置は、－Ｒ^－１ｔにより表される。

ステップＳ２４において、絶対位置計算器３４は、マーカー評価処理（図１７～図２９を参照して後述する）を実行し、マーカー４を適切な角度及び適切な距離から撮影しているか否か、すなわち、認識されたマーカー４を信頼できるか否かを判断する。認識されたマーカー４を信頼できると判断したとき、ステップＳ２５からステップＳ２６に進み、そうでないとき、図８のステップＳ４に進む。

ステップＳ２６において、絶対位置計算器３４は、ステップＳ２１において検出されたマーカー４の識別子に基づいて、記憶装置３５から、世界座標系におけるマーカー４の位置及び姿勢（すなわち、マーカー４の絶対位置及び絶対姿勢）を読み出す。

ステップＳ２７において、絶対位置計算器３４は、ステップＳ２３において計算されたマーカー座標系における撮影装置１１の位置及び姿勢と、ステップＳ２６において読み出された世界座標系におけるマーカー４の位置及び姿勢とに基づいて、世界座標系における車両１の位置及び姿勢（すなわち、車両１の絶対位置及び絶対姿勢）を計算する。マーカー座標系における撮影装置１１の位置及び姿勢に対して、世界座標系におけるマーカー４の位置及び姿勢をオフセット値として加算することにより、世界座標系における車両１の位置及び姿勢が得られる。

［補正処理］
図１４は、図８のステップＳ４（補正処理）のサブルーチンを示すフローチャートである。

図１４の補正処理を行うとき、補正器３６は、初期状態では、以前に計算された最新の絶対位置及び絶対姿勢を内部に保持している。

ステップＳ３１において、補正器３６は、車両１の相対位置及び絶対位置に基づいて、車両１の補正移動量を計算する。

図１５は、図１４のステップＳ３１において計算される補正移動量を説明する図である。図１５は、測位装置１２によって計算される車両１の位置を示す。時刻ｎ－２～ｎ＋２において、車両１は相対位置Ｐ（ｎ－２）～Ｐ（ｎ＋２）を有する。車両１が相対位置Ｐ（ｎ）に到来したとき、測位装置１２はマーカー４を検出する。このとき、マップ上では相対位置Ｐ（ｎ）がマーカー４から遠隔していても、実際には、車両１はマーカー４の近傍に位置していると考えられる。従って、測位装置１２は、相対位置Ｐ（ｎ）とマーカー４の位置との差を補正移動量τとして計算する。測位装置１２は、相対位置Ｐ（ｎ）に補正移動量τを加算して、補正された位置Ｐ’（ｎ）を得る。その後も同様に、測位装置１２は、相対位置Ｐ（ｎ＋１），Ｐ（ｎ＋２），…に補正移動量τを加算して、補正された位置Ｐ’（ｎ＋１），Ｐ’（ｎ＋２），…を得る。

図１４のステップＳ３２において、補正器３６は、車両１の補正移動量を車両１の相対位置に加算する。

ステップＳ３３において、補正器３６は、車両１の相対姿勢及び絶対姿勢に基づいて、車両１の補正回転量を計算する。

図１６は、図１４のステップＳ３３において計算される補正回転量を説明する図である。図１６は、測位装置１２によって計算された車両１の位置Ｐ，Ｐ’及び姿勢Ａ，Ａ’を示す。時刻ｎ－２～ｎ＋２において、車両１は相対位置Ｐ（ｎ－２）～Ｐ（ｎ＋２）及び相対姿勢Ａ（ｎ－２）～Ａ（ｎ＋２）を有する。図１６において、太矢印は車両１の姿勢を示す。車両１が相対位置Ｐ（ｎ）に到来したとき、測位装置１２はマーカー４を検出し、車両１の絶対姿勢を計算する。測位装置１２は、相対位置Ｐ（ｎ）を回転の中心として、相対姿勢Ａ（ｎ）と絶対姿勢との差を補正回転量ρとして計算する。測位装置１２は、相対姿勢Ａ（ｎ）に補正回転量ρを加算して、補正された姿勢Ａ’（ｎ）を得る。その後も同様に、測位装置１２は、相対姿勢Ａ（ｎ＋１），Ａ（ｎ＋２），…に補正回転量ρを加算して、補正された姿勢Ａ’（ｎ＋１），Ａ’（ｎ＋２），…を得る。

ステップＳ３３において、補正器３６は、予め決められた時間長にわたって計算された（又は、予め決められた個数の連続した画像に対応する）複数の相対姿勢の平均値と、絶対姿勢との差を、車両１の補正回転量として計算してもよい。車両１の相対姿勢の瞬時値だけでは、車両１の実際の進行方向（例えば、車両１が通路１０１に沿って移動しているのか否か）を判断することができない。また、車両１の相対姿勢は誤差を含む可能性がある。従って、複数の相対姿勢の平均値を用いることにより、車両１の実際の進行方向を正確に判断することができる。

図１４のステップＳ３４において、補正器３６は、車両１の補正回転量を車両１の相対姿勢に加算する。

［マーカー評価処理］
図１７は、図１１のステップＳ２４（マーカー評価処理）のサブルーチンを示すフローチャートである。

ステップＳ４１において、絶対位置計算器３４は、撮影装置１１によって連続的に撮影された画像であって、同一の識別子を有するマーカー４がそれぞれ抽出された画像の個数をカウントする。さらに、ステップＳ４１において、絶対位置計算器３４は、撮影装置１１によって連続的に撮影された予め決められた個数の画像からそれぞれ抽出された予め決められた個数のマーカー４が同一の識別子を有するか否かを判断し、ＹＥＳのときはステップＳ４２に進み、ＮＯのときはステップＳ４７に進む。

図１８は、図１の撮影装置１１によって撮影された画像４０の例を示す図である。倉庫又は工場のような複雑な背景を含む環境下でマーカー４を撮影する場合、何らかのオブジェクト５を、マーカーとして、すなわち、マーカーの識別子（例えば、ＩＤ：６６８）を有する視覚的に識別可能な領域として誤認識する可能性がある。特に、マーカー４のパターンがシンプルである場合、他のオブジェクト５をマーカーとして誤認識しやすくなる。

図１９は、図３の画像認識器３３によって検出されたマーカー４と、マーカー４であると誤検出されたオブジェクトとを含むテーブルである。図１９のテーブルの各行は、撮影装置１１によって撮影された各画像から抽出された、マーカー４の候補（マーカー４又は誤検出されたオブジェクト）を示す。１つの画像から１つ又は複数のマーカー４の候補が検出されることがあり、また、ある画像からマーカー４の候補が検出されないこともある。図１９のテーブルは、各マーカー４の候補について、識別子（ＩＤ）（１列目）、当該マーカー４の候補を含む画像を撮影した時刻（２列目）、位置（３～５列目）、及び角度（６列目）を示す。画像認識器３３があるマーカー４を検出しているとき、このマーカー４に近接したオブジェクト５を一時的にマーカーとして誤認識する可能性がある。しかしながら、ある画像においてオブジェクト５がマーカー４として誤検出しても、連続した複数の画像にわたって同様にオブジェクト５をマーカー４として誤検出する可能性は非常に低いと考えられる。従って、ある画像において、ある識別子（例えば、ＩＤ：６６８）を有するオブジェクト５を検出しても、それが連続した複数の画像で検出されなければ、オブジェクト５はマーカー４ではないと判断できる。一方、撮影装置１１によって連続的に撮影された複数の画像から同一の識別子（例えば、ＩＤ：０１０）のマーカー４が検出された場合、マーカー４を検出していると判断することができる。

図１７のステップＳ４１の閾値（予め決められた個数）は、例えば「３」に設定されるが、他の任意の値に設定されてもよい。

図１７のステップＳ４２において、絶対位置計算器３４は、撮影装置１１によって連続的に撮影された予め決められた個数の画像からそれぞれ抽出された同一の識別子を有する予め決められた個数のマーカー４をそれぞれ原点とする予め決められた個数のマーカー座標系において、撮影装置１１（すなわち車両１）の位置及び姿勢を計算する。さらに、ステップＳ４２において、絶対位置計算器３４は、予め決められた個数（閾値）の画像にわたって、マーカー座標系における撮影装置１１（すなわち車両１）の座標が連続して同符号を有するか否かを判断し、ＹＥＳのときはステップＳ４３に進み、ＮＯのときはステップＳ４７に進む。

図２０は、図１の撮影装置１１によって撮影され、車両１の絶対位置及び絶対姿勢の計算に適したマーカー４を含む画像４０の例を示す図である。図２１は、図１の撮影装置１１によって撮影され、車両１の絶対位置及び絶対姿勢の計算に適さないマーカー４を含む画像４０の例を示す図である。画像４０において、例えば矩形のマーカー４の対辺が互いに異なる長さを有するとき、撮影装置１１がマーカー４をどこから撮影しているのか、すなわち、マーカー座標系における撮影装置１１の位置を明確に決定することができる。例えば、図２０に示すように、マーカー４の右辺が左辺よりも長いとき、マーカー４の右側からマーカー４を撮影していると判断することができる。一方、画像４０において、例えば矩形のマーカー４の対辺が互いに同じ長さを有するとき、マーカー座標系における撮影装置１１の位置を決定することは困難である。例えば、図２１に示すように、マーカー４の右辺及び左辺の長さが同じであるとき、マーカー４の右側からマーカー４を撮影しているのか、マーカー４の左側からマーカー４を撮影しているのか、判断することができない。

図２２は、マーカー座標系における撮影装置１１の位置の反転を説明する図である。図２３の例では、マーカー４の法線に対して右側に位置した撮影装置１１ａのＸｍ座標は正であり、マーカー４の法線に対して左側に位置した撮影装置１１ｂのＸｍ座標は負である。

図２３は、図３の位置計算器３４によって計算されたマーカー座標系における撮影装置１１の位置の変化を示すテーブルである。図２３のテーブルの各行は、撮影装置１１によって撮影された各画像に対応し、各１つの画像に含まれる１つのマーカー４の識別子と、このマーカー４を原点とするマーカー座標系における撮影装置１１の位置及び姿勢とを示す。正方形のマーカー４の対辺が互いに同じ長さを有し、マーカー座標系における撮影装置１１の位置を決定することが困難である場合、マーカー座標系における撮影装置１１の座標の符号が変動しやすくなる。従って、マーカー座標系における撮影装置１１の座標の符号が頻繁に変化するとき、マーカー４を含む画像４０は、車両１の絶対位置及び絶対姿勢の計算に適さないと判断することができる。一方、予め決められた個数の画像にわたって、マーカー座標系における撮影装置１１の座標が連続して同符号を有するとき、マーカー４を含む画像４０は、車両１の絶対位置及び絶対姿勢の計算に適していると判断することができる。

図２３の例では、２～４行目において、Ｘｍ座標の符号が交互に反転しているので、対応する画像は、車両１の絶対位置及び絶対姿勢の計算に適さない。一方、４～６行目では、Ｘｍ、Ｙｍ、及びＺｍ座標が同符号を有するので、対応する画像は、車両１の絶対位置及び絶対姿勢の計算に適している。

図１７のステップＳ４２の閾値（予め決められた個数）は、例えば「３」に設定されるが、他の任意の値に設定されてもよい。

図１７のステップＳ４３において、絶対位置計算器３４は、撮影装置１１によって撮影された画像におけるマーカー４のサイズを計算する。さらに、ステップＳ４３において、絶対位置計算器３４は、撮影装置１１によって撮影された画像におけるマーカー４のサイズが予め決められた閾値より大きいか否かを判断し、ＹＥＳのときはステップＳ４４に進み、ＮＯのときはステップＳ４７に進む。

図２４は、図１の撮影装置１１によって撮影され、車両１の絶対位置及び絶対姿勢の計算に適した幅を有するマーカー４の像の例を示す図である。図２５は、図１の撮影装置１１によって撮影され、車両１の絶対位置及び絶対姿勢の計算に適した幅をもたないマーカー４の像の例を示す図である。図２４及び図２５は、撮影装置１１によって撮影された画像におけるマーカー４のサイズ（画素数）を示す。図２４及び図２５の例では、マーカー４のサイズは、各頂点からその対辺までの画素数によって表される。マーカー４のパターンを正しく認識するためには、撮影装置１１によって撮影された画像において、マーカー４が十分なサイズ（画素数）を有する必要がある。例えば、図５に示すマーカー４は、縦横に７×７個の白又は黒の正方形のセルからなるパターンを有する。この場合、図２４のマーカー４の画素数は、そのパターンの各セルを認識するために十分である。一方、図２５のマーカー４の画素数は、明らかに、そのパターンの各セルを認識するためには不十分である。

図２６は、図１７のステップＳ４３における閾値を変化させたときの車両１の絶対位置の誤差を示すグラフである。図２６によれば、閾値を１４以上に設定したとき、誤差が小さくなることがわかる。従って、撮影装置１１によって撮影された画像におけるマーカー４のサイズが予め決められた閾値より小さいとき、マーカー４を含む画像４０は、車両１の絶対位置及び絶対姿勢の計算に適さないと判断することができる。一方、撮影装置１１によって撮影された画像におけるマーカー４のサイズが予め決められた閾値より大きいとき、マーカー４を含む画像４０は、車両１の絶対位置及び絶対姿勢の計算に適していると判断することができる。

マーカー４のサイズは、各頂点からその対辺までの距離（画素数）に代えて、各辺の長さ、各辺の長さの合計、対角線の長さ、又はマーカーの面積によって表されてもよい。

図１７のステップＳ４４において、絶対位置計算器３４は、１つのマーカー４を原点とするマーカー座標系における車両１の位置及び姿勢に基づいて、１つのマーカー４の面の法線に対する車両１の方向への角度を計算する。さらに、ステップＳ４４において、絶対位置計算器３４は、マーカー座標系において１つのマーカー４の面の法線に対する撮影装置１１（すなわち車両１）の方向への角度が予め決められた閾値αｔｈより大きいか否かを判断し、ＹＥＳのときはステップＳ４５に進み、ＮＯのときはステップＳ４７に進む。なお、撮影装置１１と車両１の方向が異なる場合、絶対位置計算器３４は、撮影装置１１とマーカー４の面の法線の角度が閾値αｔｈより大きいか否かを判断し、ＹＥＳのときはステップＳ４５に進み、ＮＯのときはステップＳ４７に進む。

図２７は、マーカー座標系において、車両１の絶対位置及び絶対姿勢の計算に適したマーカー４の像を撮影可能な角度及び距離を説明する図である。ステップＳ４２に関連して前述したように、画像４０において、例えば矩形のマーカー４の対辺が互いに異なる長さを有するとき、マーカー座標系における撮影装置１１の位置を明確に決定することができる。従って、撮影装置１１は、マーカー４の法線方向（正面）からではなく、マーカー４の法線方向から十分に傾斜した方向から、マーカー４を撮影する。

図２８は、車両１の絶対位置及び絶対姿勢の計算に適したマーカー４の像を撮影するときの角度の条件を説明する図である。図２９は、図２８の撮影装置１１によって撮影されたマーカー４を含む画像の例を示す図である。図２８において、αは、マーカー４の面の法線に対する撮影装置１１の方向への角度を示す。角度αは、α＝ａｒｃｃｏｓ（ａ／ｂ）を満たす。ここで、ａ及びｂは、図２９に示すように、撮影装置１１によって撮影された画像４０におけるマーカー４のサイズを示す。ａ：ｂ＝１：１に近づくと、精度が低下する。角度αの正負は、図２９の台形の向きから判定可能である。ただし、撮影装置１１がマーカー４を正面から撮影するとき、また、撮影装置１１からマーカー４までの距離ｄが増大したとき、角度αの正負の判定は不安定になる。βは、撮影装置１１の光軸に対するマーカー４の方向の角度である。言い換えると、角度βは、撮影装置１１によって撮影された画像４０におけるマーカー４の位置を示す。撮影装置１１からマーカー４までの距離ｄは、ｄ＝１／ｂを満たす。求めた結果としては、β＞ｄ＞αの順に高精度になる。

図１７のステップＳ４４の閾値αｔｈは、例えば「４５度」に設定されるが、他の任意の値に設定されてもよい。また、この閾値αｔｈは、撮影装置１１からマーカー４までの距離に応じて変動してもよい。

マーカー４の面の法線に対する撮影装置１１の方向への角度が９０度に近づくとき、撮影装置１１によって撮影された画像におけるマーカー４のサイズａが小さくなり、ステップＳ４３の条件を満たすことが困難になる。マーカー４の面の法線に対する撮影装置１１の方向への角度は、ステップＳ４３及びＳ４４の両方の条件を満たす必要がある。

図１７のステップＳ４５において、絶対位置計算器３４は、撮影装置１１によって撮影された画像に基づいて車両１からマーカー４までの距離を計算する。さらに、ステップＳ４５において、絶対位置計算器３４は、撮影装置１１（すなわち車両１）からマーカー４までの距離が予め決められた閾値より小さいか否かを判断し、ＹＥＳのときはステップＳ４６に進み、ＮＯのときはステップＳ４７に進む。

再び図２７を参照すると、撮影装置１１は、マーカー４に対して距離Ｄｔｈより短い位置において、マーカー４を撮影する。撮影装置１１からマーカー４までの距離が大きいと、測位装置１２によって計算される車両１の位置及び姿勢の精度が低下する。

図１７のステップＳ４５の閾値Ｄｔｈは、マーカー４がＡ２サイズを有する場合、例えば「６ｍ」に設定されるが、他の任意の値に設定されてもよい。また、この閾値Ｄｔｈは、マーカー４のサイズに応じて変動してもよい。

図１７のステップＳ４６において、絶対位置計算器３４は、マーカー４を信頼できると判断する。この場合、絶対位置計算器３４は、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を補正器３６に送る。一方、ステップＳ４７において、絶対位置計算器３４は、マーカー４を信頼できないと判断する。この場合、絶対位置計算器３４は、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を送らない。

このように図１７のマーカー評価処理を実行することにより、絶対位置計算器３４は、マーカー４を適切な角度及び適切な距離から撮影しているか否か、すなわち、認識されたマーカー４を信頼できるか否かを判断する。その結果、図８のステップＳ４の補正処理において、補正器３６は以下のように動作する。

マーカー４が予め決められた閾値に等しい個数の画像にわたって同一の識別子をもたないとき（ステップＳ４１がＮＯ）、補正器３６は、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を用いて位置及び姿勢を補正しない。一方、マーカー４が閾値以上の個数の画像にわたって同一の識別子を有するとき（ステップＳ４１がＹＥＳ）、補正器３６は、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を使用して、補正された位置及び補正された姿勢を生成する。

予め決められた閾値に等しい個数の画像にわたって各マーカー座標系における車両１の座標が同じ符号をもたないとき（ステップＳ４２がＮＯ）、補正器３６は、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を用いて位置及び姿勢を補正しない。一方、閾値以上の個数の画像にわたって各マーカー座標系における車両１の座標が同じ符号を有するとき（ステップＳ４２がＹＥＳ）、補正器３６は、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を使用して、補正された位置及び補正された姿勢を生成する。

マーカー４が予め決められた閾値以下のサイズを有するとき（ステップＳ４３がＮＯ）、補正器３６は、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を用いて位置及び姿勢を補正しない。一方、マーカー４が閾値より大きいサイズを有するとき（ステップＳ４３がＹＥＳ）、補正器３６は、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を使用して、補正された位置及び補正された姿勢を生成する。

１つのマーカー４の面の法線に対する車両１の方向への角度が予め決められた閾値以下であるとき（ステップＳ４４がＮＯ）、補正器３６は、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を用いて位置及び姿勢を補正しない。一方、１つのマーカー４の面の法線に対する車両１の方向への角度が閾値より大きいとき（ステップＳ４４がＹＥＳ）、補正器３６は、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を使用して、補正された位置及び補正された姿勢を生成する。

マーカー４が車両１から予め決められた閾値以上の距離を有するとき（ステップＳ４５がＮＯ）、補正器３６は、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を用いて位置及び姿勢を補正しない。一方、マーカー４が車両１から閾値より小さい距離を有するとき（ステップＳ４５がＹＥＳ）、補正器３６は、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を使用して、補正された位置及び補正された姿勢を生成する。

本明細書では、ステップＳ４４の閾値を「第１の閾値」ともいう。また、本明細書では、ステップＳ４１の閾値を「第２の閾値」ともいう。また、本明細書では、ステップＳ４２の閾値を「第３の閾値」ともいう。また、本明細書では、ステップＳ４３の閾値を「第４の閾値」ともいう。また、本明細書では、ステップＳ４５の閾値を「第５の閾値」ともいう。

図３０は、第１の実施形態の比較例に係る補正処理を実行することによって計算された車両１の軌跡１０３を示す図である。図３１は、図１７のマーカー評価処理を実行することによって計算された車両１の軌跡１０４を示す図である。図３０～図３１では、説明のため、各マーカー４の一辺に設けた突起によりマーカー４の正面（図５のＺｍ軸）を示すが、実際には、このような突起はない。図３０によれば、領域１１１、１１３、及び１１４において、マーカー座標系における撮影装置１１の位置を正しく決定できなかったことに起因して、図２２に示すように撮影装置１１の位置がマーカー４の法線に対して反転し、計算された車両１の位置の誤差が生じている。また、図３０によれば、領域１１２、１１５、及び１１６において、計算された車両１の位置の精度が低下している。また、領域１１７では、マーカー４ではないオブジェクトをマーカー４として誤検出したことにより、大幅な誤差が生じている。一方、図３１によれば、測位装置１２は、適切な角度及び適切な距離から撮影されたマーカー４に基づいて計算された絶対位置及び絶対姿勢のみに基づいて相対位置及び相対姿勢を補正しているので、車両１の位置及び姿勢を高精度に測定することができる。

第１の実施形態によれば、Visual-SLAMなどを用いて特徴点４１に基づいて計算された相対位置及び相対姿勢を、マーカー４に基づいて計算された絶対位置及び絶対姿勢に基づいて補正することにより、車両１の位置及び姿勢を高精度に測定することができる。また、第１の実施形態によれば、認識されたマーカー４を信頼できるか否かを判断し、マーカー４を信頼できるときのみ、絶対位置及び絶対姿勢に基づいて車両１の位置及び姿勢を補正することにより、車両１の位置及び姿勢をより高精度に測定することができる。

第１の実施形態によれば、撮影装置１１を用いることにより、倉庫又は工場など、ＧＰＳ衛星からの電波を受信できない屋内であっても、車両１の位置を安価に測定することができる。無線信号を送信するための多数の無線送信機を設置する必要がないので、初期導入コストを低減できる。

第１の実施形態によれば、測位結果から得られる車両１の移動経路に基づいて、業務改善を行うことができる。

第１の実施形態によれば、車両１の移動距離に基づいて、保守の要否、リース契約の更改の要否、などに活用することができる。

第１の実施形態によれば、車両１の移動のヒートマップに基づいて、倉庫又は工場における通路又は棚などの配置を最適化することができる。

第１の実施形態によれば、各車両１の軌跡の違いに基づいて、各車両１が移動中に互いにすれ違う場所を可視化することができ、経路及び通路幅を見直して安全性を向上させることができる。

［第１の実施形態の変形例］
図１７のステップＳ４１～Ｓ４５は、他の任意の順序で実行されてもよい。処理の順序によって結果は変わらないが、処理速度に違いが生じる。例えば、ステップＳ４４よりも前にステップＳ４２を実行することにより、撮影装置１１がマーカー４の正面に位置する場合を効率的に除去することができる。また、ステップＳ４１～Ｓ４５のうちの一部のみが実行されてもよい。

図１１のステップＳ２４～Ｓ２５は、絶対位置計算器３４に代えて、補正器３６によって実行されてもよい。この場合、絶対位置計算器３４は、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を常に補正器３６に送る。補正器３６は、マーカー４を信頼できるときのみ、絶対位置及び絶対姿勢に基づいて車両１の位置及び姿勢を補正する。

上述した例では、補正器３６が、相対位置及び相対姿勢を絶対位置及び絶対姿勢によって上書きせず、絶対位置及び絶対姿勢を相対位置及び相対姿勢と別個に管理する場合について説明した。それに代わって、補正器３６が相対位置及び相対姿勢を絶対位置及び絶対姿勢によって上書きし、その後、この絶対位置及び絶対姿勢を新たな基準位置及び基準姿勢として、相対位置及び相対姿勢を計算してもよい。

撮影装置１１は、被写体の画像を生成するとともに、撮影装置１１から被写体の各点までの距離を検出するように構成されてもよい。撮影装置１１は、被写体までの距離を検出するため、例えば、ＲＧＢ－Ｄカメラなどのデプスセンサ、又はＴｏＦ（Ｔｉｍｅ ｏｆ Ｆｌｉｇｈｔ）センサを含んでもよい。それに代わって、撮影装置１１は、被写体までの距離を検出するため、所定距離だけ互いに離れた配置された２つのカメラを含むステレオカメラであってもよい。

撮影装置１１が距離を検出する場合、相対位置計算器３２は、公知のＩＣＰ（iterative closest point）アルゴリズムなどを用いて、車両１の相対位置及び相対姿勢を計算してもよい。

図９の相対位置計算処理では、ＦＡＳＴ及びＫＬＴトラッカーを用いる例を説明したが、他の方法を用いてもよい。例えば、画像処理で一般的であるＳＩＦＴ（Ｓｃａｌｅ Ｉｎｖａｒｉａｎｔ Ｆｅａｔｕｒｅ Ｔｒａｎｓｆｏｒｍ）又はＯＲＢ（Ｏｒｉｅｎｔｅｄ ＦＡＳＴ ａｎｄ Ｒｏｔａｔｅｄ ＢＲＩＥＦ）を用いた特徴点検出処理及び特徴点マッチング処理などを用いてもよい。

マーカー４が通路１０１の直線区間の中間に配置されている場合、絶対位置及び絶対姿勢を高精度に計算できると期待される。一方、マーカー４が通路１０１の交差点又は出入口の付近に配置されている場合、車両１はマーカー４の近傍を直進するとは限らず、計算された絶対位置及び絶対姿勢の誤差が大きくなる可能性がある。従って、複数のマーカー４のうちの一部は、絶対位置及び絶対姿勢を計算するために（すなわち、位置及び姿勢を補正するために）使用されない補助マーカーであってもよい。補助マーカーは、例えば、車両１の出発地、目的地、又は他のチェックポイントとなりうる何らかの構造物（倉庫１００の出入口、通路１０１の交差点、特定の棚１０２、など）の近傍に配置される。補助マーカーは、車両１から撮影可能であるならば、通路１０１に沿って配置されなくてもよい。測位装置１２は、補助マーカーを検出することにより、車両１が特定のチェックポイントに到達したことを認識することができる。この場合、記憶装置３５に格納されたマーカー情報のテーブルは、各マーカー４が補助マーカーであるか否かを示す項目をさらに含む。また、この場合、マーカー情報のテーブルは、補助マーカーについては、その位置及び姿勢の情報をもたなくてもよい。また、マーカー情報のテーブルは、各マーカー４が補助マーカーであるか否かを示す項目に代えて、当該マーカー４に基づいて計算される絶対位置及び絶対姿勢の信頼度を示す項目を含んでもよい。

車両１及びサーバ装置２は、通信装置１３、２２に代えて、ＳＤカードなど、着脱可能な記憶媒体を用いてもよい。車両において計算した当該車両の位置及び姿勢を記憶媒体に書き込み、サーバ装置２において、この記憶媒体から車両の位置及び姿勢を読み出してもよい。

車両１は、例えば無人搬送車（ＡＧＶ）又はパレット搬送ロボットなど、無人の荷物運搬装置であってもよい。この場合、車両１は、サーバ装置２の制御下で駆動機構１５を制御して移動する。

［第１の実施形態の効果等］
第１の実施形態によれば、測位装置１２は、相対位置計算器３２、記憶装置３５、絶対位置計算器３４、及び補正器３６を備える。相対位置計算器３２は、車両１に搭載された撮影装置１１によって撮影された複数の画像に基づいて、所定の基準位置及び基準姿勢に対する車両１の相対位置及び相対姿勢を計算する。記憶装置３５は、予め決められた位置に配置されて視覚的に識別可能な複数のマーカー４の識別子、位置、及び姿勢の情報と、車両１のための通路を含むマップの情報とを格納する。絶対位置計算器３４は、撮影装置１１によって撮影された画像から複数のマーカー４のうちの１つを抽出し、抽出された１つのマーカー４の位置及び姿勢に基づいて、マップにおける車両１の位置及び姿勢を示す車両１の絶対位置及び絶対姿勢を計算する。また、絶対位置計算器３４は、１つのマーカー４を原点とするマーカー座標系における車両１の位置及び姿勢に基づいて、１つのマーカー４の面の法線に対する車両１の方向への角度を計算する。補正器３６は、絶対位置及び絶対姿勢に基づいて相対位置及び相対姿勢を補正し、移動体の補正された位置及び補正された姿勢を生成する。補正器３６は、角度が第１の閾値以下であるマーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を使用せず、角度が第１の閾値より大きいマーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を使用して、補正された位置及び補正された姿勢を生成する。

これにより、補正器３６は、マーカー４の面の法線に対する車両１の方向への角度が第１の閾値より大きいときのみ、マーカー４の位置及び姿勢に基づいて車両１の位置及び姿勢を補正する。

第１の実施形態によれば、絶対位置計算器３４は、撮影装置１１によって連続的に撮影された画像であって、同一の識別子を有するマーカー４がそれぞれ抽出された画像の個数をカウントしてもよい。この場合、補正器３６は、マーカー４が第２の閾値に等しい個数の画像にわたって同一の識別子をもたないとき、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を使用せず、マーカー４が第２の閾値以上の個数の画像にわたって同一の識別子を有するとき、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を使用して、補正された位置及び補正された姿勢を生成する。

これにより、補正器３６は、マーカー４が第２の閾値以上の個数の画像にわたって同一の識別子を有するときのみ、マーカー４の位置及び姿勢に基づいて車両１の位置及び姿勢を補正する。

第１の実施形態によれば、絶対位置計算器３４は、撮影装置１１によって連続的に撮影された複数の画像からそれぞれ抽出された同一の識別子を有する複数のマーカー４をそれぞれ原点とする各マーカー座標系において、車両１の位置及び姿勢を計算してもよい。この場合、補正器３６は、第３の閾値に等しい個数の画像にわたって各マーカー座標系における車両１の座標が同じ符号をもたないとき、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を使用せず、第３の閾値以上の個数の画像にわたって各マーカー座標系における車両１の座標が同じ符号を有するとき、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を使用して、補正された位置及び補正された姿勢を生成する。

これにより、補正器３６は、第３の閾値以上の個数の画像にわたって各マーカー座標系における車両１の座標が同じ符号を有するときのみ、マーカー４の位置及び姿勢に基づいて車両１の位置及び姿勢を補正する。

第１の実施形態によれば、絶対位置計算器３４は、撮影装置１１によって撮影された画像におけるマーカー４のサイズを計算してもよい。この場合、補正器３６は、第４の閾値以下のサイズを有するマーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を使用せず、第４の閾値より大きいサイズを有するマーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を使用して、補正された位置及び補正された姿勢を生成する。

これにより、補正器３６は、マーカー４が第４の閾値より大きいサイズを有するときのみ、マーカー４の位置及び姿勢に基づいて車両１の位置及び姿勢を補正する。

第１の実施形態によれば、絶対位置計算器３４は、撮影装置１１によって撮影された画像に基づいて車両１からマーカー４までの距離を計算してもよい。この場合、補正器３６は、車両１から第５の閾値以上の距離を有するマーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を使用せず、車両１から第５の閾値より小さい距離を有するマーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を使用して、補正された位置及び補正された姿勢を生成する。

これにより、補正器３６は、マーカー４が車両１から第５の閾値より小さい距離を有するときのみ、マーカー４の位置及び姿勢に基づいて車両１の位置及び姿勢を補正する。

第１の実施形態によれば、測位装置１２は、相対位置計算器３２、記憶装置３５、絶対位置計算器３４、及び補正器３６を備える。相対位置計算器３２は、車両１に搭載された撮影装置１１によって撮影された複数の画像に基づいて、所定の基準位置及び基準姿勢に対する車両１の相対位置及び相対姿勢を計算する。記憶装置３５は、予め決められた位置に配置されて視覚的に識別可能な複数のマーカー４の識別子、位置、及び姿勢の情報と、車両１のための通路を含むマップの情報とを格納する。絶対位置計算器３４は、撮影装置１１によって撮影された画像から複数のマーカー４のうちの１つを抽出し、抽出された１つのマーカー４の位置及び姿勢に基づいて、マップにおける車両１の位置及び姿勢を示す車両１の絶対位置及び絶対姿勢を計算する。また、絶対位置計算器３４は、撮影装置１１によって連続的に撮影された画像であって、同一の識別子を有するマーカー４がそれぞれ抽出された画像の個数をカウントする。補正器３６は、絶対位置及び絶対姿勢に基づいて相対位置及び相対姿勢を補正し、移動体の補正された位置及び補正された姿勢を生成する。補正器３６は、マーカー４が第２の閾値に等しい個数の画像にわたって同一の識別子をもたないとき、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を使用せず、マーカー４が第２の閾値以上の個数の画像にわたって同一の識別子を有するとき、マーカー４の位置及び姿勢に基づいて計算される絶対位置及び絶対姿勢を使用して、補正された位置及び補正された姿勢を生成する。

これにより、補正器３６は、マーカー４が第２の閾値以上の個数の画像にわたって同一の識別子を有するときのみ、マーカー４の位置及び姿勢に基づいて車両１の位置及び姿勢を補正する。

第１の実施形態によれば、撮影装置１１と、測位装置１２とを備えた車両１が提供されてもよい。

第１の実施形態によれば、Visual-SLAMなどを用いて特徴点４１に基づいて計算された相対位置及び相対姿勢を、マーカー４に基づいて計算された絶対位置及び絶対姿勢に基づいて補正することにより、車両１の位置及び姿勢を高精度に測定することができる。また、第１の実施形態によれば、認識されたマーカー４を信頼できるか否かを判断し、マーカー４を信頼できるときのみ、絶対位置及び絶対姿勢に基づいて車両の位置及び姿勢を補正することにより、車両１の位置及び姿勢をより高精度に測定することができる。

［第２の実施形態］
図３２を参照して、第２の実施形態に係る測位装置及びそれを備えた移動体について説明する。

［第２の実施形態の構成］
図３２は、第２の実施形態に係る測位装置１２Ｂの構成を示すブロック図である。第１の実施形態に係る車両１は、図３の測位装置１２に代えて、測位装置１２Ｂを備えてもよい。測位装置１２Ｂは、図３の測位装置１２の各構成要素に加えて、画像認識器３７及びデータ合成器３８を備える。

画像認識器３７は、撮影装置１１によって撮影された画像から予め決められた対象物を認識する。画像認識器３７は人物（例えば、車両１の運転手、車両１の周りの人物）を認識してもよい。画像認識器３７は、予め学習された特定の荷物３を認識してもよい。画像処理器３１、画像認識器３３、及び画像認識器３７は、同一の撮影装置１１から画像を取得してもよい。それに代わって、画像認識器３７は、画像処理器３１及び画像認識器３３に供給される画像（すなわち、車両１の位置及び姿勢を測定するための画像）を撮影する撮影装置１１とは異なる撮影装置を用いて、人物及び／又は荷物３を含む画像を撮影してもよい。この場合、撮影装置１１は、例えば、車両１の前方の通路１０１を撮影するように設けられ、他の撮影装置は、例えば、車両１の運転席手又は荷台を撮影するように設けられてもよい。撮影装置１１と他の撮影装置とは、予め互いに同期される。

データ合成器３８は、補正器３６から、車両１の補正された位置及び補正された姿勢のデータを、当該位置及び姿勢に対応する画像を撮影装置１１により撮影した時刻（又は、当該位置及び姿勢を計算した時刻）のタイムスタンプとともに取得する。データ合成器３８は、画像認識器３７によって認識された対象物の画像認識データを、車両１の位置及び姿勢のデータに合成する。また、データ合成器３８は、車両１に搭載された１つ又は複数のセンサを含むセンサ群１６Ｂによって生成されたセンサデータを取得し、センサデータを、車両１の位置及び姿勢のデータに合成する。データ合成器３８は、車両１の加速度及び角速度の少なくとも一方を含むセンサデータを取得してもよい。また、データ合成器３８は、車両１によって搬送される荷物３の重さを含むセンサデータを取得してもよい。センサ群１６Ｂの各センサは、センサデータに、当該センサデータを取得した時刻のタイムスタンプを付与する。データ合成器３８は、車両１の位置及び姿勢のデータのタイムスタンプと、画像認識データのタイムスタンプと、センサデータのタイムスタンプとに基づいて、これらのデータを互いに同期させて合成する。

データ合成器３８は、これらのデータのタイムスタンプが互いに不一致しているとき、画像認識データ又はセンサデータを、それに最も近接したタイムスタンプを有する車両１の位置及び姿勢のデータに関連付けてもよい。また、データ合成器３８は、これらのデータのタイムスタンプが互いに不一致しているとき、線形補間又は内分などを用いて車両１の位置及び姿勢のデータを補間し、画像認識データ又はセンサデータを、それに対応するタイムスタンプを有する補間された車両１の位置及び姿勢のデータに関連付けてもよい。

データ合成器３８を備えたことにより、車両１の作業に関連するさまざまなデータを、車両１の位置及び軌跡に関連付けて記録することができる。例えば、画像認識された人物を記録することにより、車両１の作業に関連する人物を追跡することができる。画像認識された荷物３を記録することにより、車両１によって搬送される荷物３を追跡することができる。車両１の加速度及び角速度を記録することにより、倉庫の路面の凹凸などを検出することができる。荷物３の重さを記録することにより、車両１の仕事量をモニタリングすることができる。

［第２の実施形態の効果等］
第２の実施形態によれば、車両１に搭載された１つ又は複数のセンサによって生成されたセンサデータを取得し、センサデータを、補正された位置及び補正された姿勢のデータに合成するデータ合成器３８をさらに備えてもよい。

第２の実施形態によれば、データ合成器３８は、車両１の加速度及び角速度の少なくとも一方を含むセンサデータを取得してもよい。

第２の実施形態によれば、データ合成器３８は、車両１によって搬送される荷物３の重さを含むセンサデータを取得してもよい。

第２の実施形態によれば、測位装置１２Ｂは、撮影装置１１によって撮影された画像から予め決められた対象物を認識する画像認識器３７をさらに備えてもよい。この場合、データ合成器３８は、画像認識器３７によって認識された対象物の情報を、補正された位置及び補正された姿勢のデータに合成する。

第２の実施形態によれば、画像認識器３７は人物を認識してもよい。

第２の実施形態によれば、画像認識器３７は、予め学習された特定の荷物３を認識してもよい。

第２の実施形態によれば、車両１の作業に関連するさまざまなデータを、車両１の位置及び軌跡に関連付けて記録することができる。

第２の実施形態によれば、車両１は、車両１の位置及び姿勢を測定するための画像を撮影する撮影装置１１と、他の対象物を撮影する他の撮影装置とを備えてもよい。この場合、データ合成器３８は、撮影装置１１によって撮影された画像に基づいて生成された車両１の位置及び姿勢のデータと、他の撮影装置によって撮影された画像に基づいて生成された画像認識データとを、互いに関連付けることができる。車両１の位置及び姿勢のデータと、車両１の移動中に撮影された他の対象物の画像認識データとが関連付けられていると、マップにおける車両１の位置及び軌跡に基づいて業務分析を行う際に非常に有用である。例えば、ある人物の不審な行動を目視等で検出したとき、その人物の位置をマップにおいて参照することで、その位置の近傍において、及び／又は、その人物に関連して、過去に撮影された画像又は映像を検索して読み出すことができる。

［他の実施形態］
なお、第１～第３の実施形態において、測位装置は、フォークリフト又はトラックなどの四輪の車両に設けられてもよく、１～３輪、５輪以上の車両に設けられてもよい。また、第１～第３の実施形態において、測位装置は、車輪の個数及び／又は車輪の有無と関係なく、飛行機、ヘリコプタ、ドローン、ホバークラフトなど、車輪のない移動体に設けられてもよい。本実施形態に係る測位装置は、車輪の回転数から移動体の位置を推定するのでなく、撮影装置によって撮影された画像に基づいて移動体の位置を推定することができる。

本開示の各態様に係る測位装置によれば、倉庫又は工場などにおいて移動体の位置を測定することができる。これにより、移動体の軌跡（動線）を追跡すること、移動体をルーティングすること、倉庫又は工場における荷物などの配置を最適化すること、稼働率をモニタリングすること、作業効率を改善すること、などが可能である。

１…車両、
１ａ…荷台、
１ｂ…昇降機構、
１ｃ…コンソール、
２…サーバ装置、
３…荷物、
４…マーカー、
１１…撮影装置、
１２，１２Ｂ…測位装置、
１３…通信装置、
１４…表示装置、
１５…駆動機構、
１６Ｂ…センサ群、
２１…処理装置、
２２…通信装置、
２３…入力装置、
２４…記憶装置、
２５…表示装置、
３１…画像処理器（特徴点）、
３２…相対位置計算器、
３３…画像認識器（マーカー）、
３４…絶対位置計算器、
３５…記憶装置、
３６，３６Ａ…補正器、
３７…画像認識器（人物、荷物）、
３８…データ合成器、
４０…画像、
４１…特徴点、
１００…倉庫、
１０１…通路、
１０２…棚、
１０３，１０４…軌跡。

Claims (17)

1. 移動体に搭載された撮影装置によって撮影された複数の画像に基づいて、所定の基準位置及び基準姿勢に対する前記移動体の相対位置及び相対姿勢を示す前記移動体の第１の位置及び第１の姿勢を計算する第１の計算器と、
   予め決められた位置に配置されて１つの面をそれぞれ有する視覚的に識別可能な複数の正方形のマーカーの識別子、位置、及び姿勢の情報と、前記移動体のための通路を含むマップの情報とを格納する記憶装置と、
   前記撮影装置によって撮影された画像から前記複数のマーカーのうちの１つを抽出し、抽出された前記１つのマーカーの位置及び姿勢に基づいて、前記マップにおける前記移動体の位置及び姿勢を示す前記移動体の第２の位置及び第２の姿勢を計算し、前記１つのマーカーを原点とするマーカー座標系における前記移動体の位置及び姿勢に基づいて、前記１つのマーカーの面の法線に対する前記撮影装置の方向への角度を計算する第２の計算器と、
   前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正し、前記移動体の第３の位置及び第３の姿勢を生成する補正器とを備え、
   前記補正器は、前記計算された角度が第１の閾値以下である場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正することなく、前記第１の位置及び前記第１の姿勢を前記第３の位置及び前記第３の姿勢として生成し、前記計算された角度が前記第１の閾値より大きい場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正し、前記第３の位置及び前記第３の姿勢を生成する、
   測位装置。
2. 前記第２の計算器は、前記撮影装置によって連続的に撮影された画像であって、同一の識別子を有するマーカーがそれぞれ抽出された画像の個数をカウントし、
   前記補正器は、前記マーカーが第２の閾値に等しい個数の画像にわたって同一の識別子をもたない場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正することなく、前記第１の位置及び前記第１の姿勢を前記第３の位置及び前記第３の姿勢として生成し、前記マーカーが前記第２の閾値以上の個数の画像にわたって同一の識別子を有する場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正し、前記第３の位置及び前記第３の姿勢を生成する、
   請求項１記載の測位装置。
3. 前記第２の計算器は、前記撮影装置によって連続的に撮影された複数の画像からそれぞれ抽出された同一の識別子を有する複数のマーカーをそれぞれ原点とする各マーカー座標系において、前記移動体の位置及び姿勢を計算し、
   前記補正器は、第３の閾値に等しい個数の画像にわたって前記各マーカー座標系における前記移動体の座標が同じ符号をもたない場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正することなく、前記第１の位置及び前記第１の姿勢を前記第３の位置及び前記第３の姿勢として生成し、前記第３の閾値以上の個数の画像にわたって前記各マーカー座標系における前記移動体の座標が同じ符号を有する場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正し、前記第３の位置及び前記第３の姿勢を生成する、
   請求項１又は２記載の測位装置。
4. 前記第２の計算器は、前記撮影装置によって撮影された画像における前記マーカーのサイズを計算し、
   前記補正器は、前記マーカーが第４の閾値以下のサイズを有する場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正することなく、前記第１の位置及び前記第１の姿勢を前記第３の位置及び前記第３の姿勢として生成し、前記マーカーが前記第４の閾値より大きいサイズを有する場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正し、前記第３の位置及び前記第３の姿勢を生成する、
   請求項１～３のうちの１つに記載の測位装置。
5. 前記第２の計算器は、前記撮影装置によって撮影された画像に基づいて前記移動体から前記マーカーまでの距離を計算し、
   前記補正器は、前記移動体から前記マーカーまでの距離が第５の閾値以上である場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正することなく、前記第１の位置及び前記第１の姿勢を前記第３の位置及び前記第３の姿勢として生成し、前記移動体から前記マーカーまでの距離が前記第５の閾値より小さい場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正し、前記第３の位置及び前記第３の姿勢を生成する、
   請求項１～４のうちの１つに記載の測位装置。
6. 移動体に搭載された撮影装置によって撮影された複数の画像に基づいて、所定の基準位置及び基準姿勢に対する前記移動体の相対位置及び相対姿勢を示す前記移動体の第１の位置及び第１の姿勢を計算する第１の計算器と、
   予め決められた位置に配置されて視覚的に識別可能な複数のマーカーの識別子、位置、及び姿勢の情報と、前記移動体のための通路を含むマップの情報とを格納する記憶装置と、
   前記撮影装置によって撮影された画像から前記複数のマーカーのうちの１つを抽出し、抽出された前記１つのマーカーの位置及び姿勢に基づいて、前記マップにおける前記移動体の位置及び姿勢を示す前記移動体の第２の位置及び第２の姿勢を計算し、前記撮影装置によって連続的に撮影された画像であって、同一の識別子を有するマーカーがそれぞれ抽出された画像の個数をカウントする第２の計算器と、
   前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正し、前記移動体の第３の位置及び第３の姿勢を生成する補正器とを備え、
   前記補正器は、前記マーカーが第２の閾値に等しい個数の画像にわたって同一の識別子をもたない場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正することなく、前記第１の位置及び前記第１の姿勢を前記第３の位置及び前記第３の姿勢として生成し、前記マーカーが前記第２の閾値以上の個数の画像にわたって同一の識別子を有する場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正し、前記第３の位置及び前記第３の姿勢を生成する、
   測位装置。
7. 前記第２の計算器は、前記撮影装置によって連続的に撮影された複数の画像からそれぞれ抽出された同一の識別子を有する複数のマーカーをそれぞれ原点とする各マーカー座標系において、前記移動体の位置及び姿勢を計算し、
   前記補正器は、第３の閾値に等しい個数の画像にわたって前記各マーカー座標系における前記移動体の座標が同じ符号をもたない場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正することなく、前記第１の位置及び前記第１の姿勢を前記第３の位置及び前記第３の姿勢として生成し、前記第３の閾値以上の個数の画像にわたって前記各マーカー座標系における前記移動体の座標が同じ符号を有する場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正し、前記第３の位置及び前記第３の姿勢を生成する、
   請求項６記載の測位装置。
8. 前記第２の計算器は、前記撮影装置によって撮影された画像における前記マーカーのサイズを計算し、
   前記補正器は、前記マーカーが第４の閾値以下のサイズを有する場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正することなく、前記第１の位置及び前記第１の姿勢を前記第３の位置及び前記第３の姿勢として生成し、前記マーカーが前記第４の閾値より大きいサイズを有する場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正し、前記第３の位置及び前記第３の姿勢を生成する、
   請求項６又は７記載の測位装置。
9. 前記第２の計算器は、前記撮影装置によって撮影された画像に基づいて前記移動体から前記マーカーまでの距離を計算し、
   前記補正器は、前記移動体から前記マーカーまでの距離が第５の閾値以上である場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正することなく、前記第１の位置及び前記第１の姿勢を前記第３の位置及び前記第３の姿勢として生成し、前記移動体から前記マーカーまでの距離が前記第５の閾値より小さい場合、前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正し、前記第３の位置及び前記第３の姿勢を生成する、
   請求項６～８のうちの１つに記載の測位装置。
10. 前記移動体に搭載された１つ又は複数のセンサによって生成されたセンサデータを取得し、前記センサデータを、前記第３の位置及び前記第３の姿勢のデータに合成するデータ合成器をさらに備えた、
    請求項１～９のうちの１つに記載の測位装置。
11. 前記データ合成器は、前記移動体の加速度及び角速度の少なくとも一方を含むセンサデータを取得する、
    請求項１０記載の測位装置。
12. 前記データ合成器は、前記移動体によって搬送される荷物の重さを含むセンサデータを取得する、
    請求項１０又は１１記載の測位装置。
13. 前記測位装置は、前記撮影装置によって撮影された画像から予め決められた対象物を認識する画像認識器をさらに備え、
    前記データ合成器は、前記画像認識器によって認識された対象物の情報を、前記第３の位置及び前記第３の姿勢のデータに合成する、
    請求項１０～１２のうちの１つに記載の測位装置。
14. 前記画像認識器は人物を認識する、
    請求項１３記載の測位装置。
15. 前記画像認識器は、予め学習された特定の荷物を認識する、
    請求項１３記載の測位装置。
16. 撮影装置と、
    請求項１～１５のうちの１つに記載の測位装置とを備えた、
    移動体。
17. 前記補正器は、前記第１の位置及び前記第１の姿勢と、当該第１の位置及び当該第１の姿勢が計算された画像と同じ画像から計算された前記第２の位置及び前記第２の姿勢とに基づいて、前記第３の位置及び前記第３の姿勢を生成する、
    請求項１～１５のうちの１つに記載の測位装置。

Images