JP7336753B2 - 測位装置及び移動体 - 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などを用いて特徴点４１に基づいて計算された相対位置及び相対姿勢を、マーカー４に基づいて計算された絶対位置及び絶対姿勢に基づいて補正することにより、車両１の位置及び姿勢を高精度に測定することができる。また、絶対位置及び絶対姿勢に基づいて相対位置及び相対姿勢を補正するとき、通路１０１の向きに基づいて車両１の相対姿勢を補正することにより、絶対姿勢の誤差による影響を低減し、車両１の位置及び姿勢をより高精度に測定することができる。

第１の実施形態によれば、補正器３６は、第２の姿勢の向きと通路１０１の向きとの角度差が閾値よりも小さいとき、第２の姿勢に代えて通路１０１の向きに基づいて第１の姿勢を補正してもよい。

これにより、補正器３６は、車両１の進行方向として車両１の絶対姿勢を参照し、車両１の絶対姿勢に基づいて、車両１が通路１０１に沿って移動しているか否かを判断することができる。

第１の実施形態によれば、補正器３６は、補正された位置を有する車両１の直近の移動を示すベクトルの向きと、通路１０１の向きとの角度差が閾値よりも小さいとき、第２の姿勢に代えて通路１０１の向きに基づいて第１の姿勢を補正してもよい。

これにより、補正器３６は、車両１の進行方向として、車両１の姿勢を参照するのではなく、車両１の位置から計算されたベクトルを参照することができる。このようなベクトルを用いることで、例えばトラックなど、車両１が短時間に急速に旋回できない場合に、車両１の進行方向を高精度に表現することができる。補正器３６は、このようなベクトルに基づいて、車両１が通路１０１に沿って移動しているか否かを高精度に判断することができる。

第１の実施形態によれば、補正器３６は、予め決められた時間長にわたって、又は、予め決められた個数の連続した画像に対応して、補正された位置を有する車両１の直近の移動を示すベクトルの向きを計算してもよい。この場合、補正器３６は、複数のベクトルの向きの平均値と、通路１０１の向きとの角度差が閾値よりも小さいとき、第２の姿勢に代えて通路１０１の向きに基づいて第１の姿勢を補正してもよい。

これにより、補正器３６は、車両１の進行方向として、車両１の位置から計算された複数のベクトルの平均値を参照することができる。このようなベクトルの平均値を用いることで、個別のベクトルの誤差の影響を受けにくくすることができる。補正器３６は、このようなベクトルの平均値に基づいて、車両１が通路１０１に沿って移動しているか否かを高精度かつ安定的に判断することができる。

第１の実施形態によれば、補正器３６は、予め決められた時間長にわたって、又は、予め決められた個数の連続した画像に対応してそれぞれ計算された複数の相対姿勢の平均値を、絶対姿勢又は通路１０１の向きに基づいて補正してもよい。

これにより、補正器３６は、このような相対姿勢の平均値を用いることで、個別の相対姿勢の誤差の影響を受けにくくすることができ、従って、車両１の位置及び姿勢をより高精度かつ安定的に測定することができる。

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

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

補正器３６は、前述のように、Visual-SLAMなどを用いて特徴点４１に基づいて計算された相対位置及び相対姿勢を、マーカー４に基づいて計算された絶対位置及び絶対姿勢に基づいて補正する。しかしながら、車両１に搭載された撮影装置１１によって撮影される画像の内容が急速に変化しているとき、相対位置及び相対姿勢の誤差が増大し、また、絶対位置及び絶対姿勢の誤差も増大すると考えられる。例えば、車両１が通路のコーナーを通過しているとき、車両１が通路の直線区間を移動しているときと比較して、これらの誤差が増大すると考えられる。また、測位装置１２によって計算される車両１の位置及び姿勢、特に、相対姿勢及び絶対姿勢の誤差の影響を受けやすいと考えられる。

このため、第２の実施形態に係る測位装置１２では、補正器３６は、車両１が旋回中であるか否かを判断し、車両１が旋回中である期間に計算された絶対姿勢を使用せず、車両１が旋回中ではない期間に計算された絶対姿勢のみを使用して、補正された姿勢を生成する。誤差を含む可能性がある絶対姿勢に基づいて、誤差を含む可能性がある相対姿勢を補正すると、補正された姿勢の信頼性がさらに悪化する。第２の実施形態によれば、車両１が旋回中である期間に計算された絶対姿勢を補正に使用しないことにより、車両１の位置及び姿勢の精度を悪化させにくくすることができる。

本開示において、「車両１が旋回中である」とは、車両１のヨー軸の周りの回転角（もしくはその分散）又は角速度が、予め決められた閾値よりも大きいことを意味する。ここで、ヨー軸とは、前述のように、床面に垂直な軸（すなわち、図４のＺｗ軸）を示す。車両１のヨー軸の周りの回転角は、車両１の長手方向と車両１の進行方向との間の角度として定義される（すなわち、車両１の座標系において定義される）。また、車両１のヨー軸の周りの角速度は、床面に対する車両１の角速度として定義される（すなわち、静止座標系において定義される）。補正器３６は、補正された姿勢を生成するために、車両１が直進している期間に計算された絶対姿勢を使用するだけでなく、閾値よりも小さな回転角又は角速度で車両１が旋回中である期間に計算された絶対姿勢を使用してもよい。

［第２の実施形態の構成］
第２の実施形態に係る測位装置１２は、第１の実施形態に係る測位装置１２と同様に構成される。第２の実施形態では、測位装置１２によって実行される補正処理が、第１の実施形態のものとは異なる。

［第２の実施形態の動作］
図２５は、第２の実施形態に係る測位装置１２によって実行される補正処理を示すフローチャートである。図２５の補正処理のステップＳ４Ａは、図８の補正処理のステップＳ４に代えて実行される。

図２５の補正処理を行うとき、補正器３６は、初期状態では、以前に計算された最新の補正移動量及び補正回転量を内部に保持している。

ステップＳ４１において、補正器３６は、車両１の相対位置及び絶対位置に基づいて、車両１の補正移動量を計算する。ステップＳ４２において、補正器３６は、車両１の補正移動量を車両１の相対位置に加算する。図２５のステップＳ４１～Ｓ４２は、第１の実施形態に係る図１４のステップＳ３１～Ｓ３２と同様である。

ステップＳ４３において、補正器３６は、車両１が旋回中であるか否かを判断し、ＹＥＳのときはステップＳ４５に進み、ＮＯのときはステップＳ４４に進む。

図２６は、第２の実施形態に係る測位装置１２を備えた車両１が移動する通路１１１を含む倉庫１１０のマップである。倉庫１１０は、複数の通路１１１及び複数の棚１１２などの構造物を含む。図２６では、図示の簡単化のために、マーカー４を省略している。図２６は、車両１が点Ａから点Ｂまで実際に移動した軌跡１１３を示す。例えば、車両１が通路１１１のコーナーを通過しているとき、車両１の向きが急速に変化、すなわち旋回中になる。また、例えば、車両１が棚１１２に対して荷物３を出し入れしているとき、車両１が右折、左折、前進、及び後退を繰り返し、車両１の向きが急速に変化、すなわち旋回中になる。

補正器３６は、例えば、相対姿勢の変化に基づいて、車両１が旋回中であるか否かを判断してもよい。補正器３６は、相対姿勢の変化に基づいて、車両１のヨー軸の周りの回転角又はその分散を計算し、回転角又はその分散の計算値が予め決められた閾値よりも大きいとき、車両１が旋回中であると判断してもよい。車両１のヨー軸の周りの回転角は、例えば、撮影装置１１によって撮影された複数の画像から特徴点を抽出し、抽出された特徴点を画像間で対応付けし、画像間の特徴点の変化量に基づいて、所定の基準姿勢に対する車両１の相対姿勢で計算することにより得られる。

図２７は、第２の実施形態に係る測位装置１２を備えた車両１の直進を説明する図である。図２８は、第２の実施形態に係る測位装置１２を備えた車両１の旋回を説明する図である。車両１が直進しているとき、ヨー軸の周りの回転角はほぼ一定であり、その分散は小さい。一方、車両１が旋回しているとき、ヨー軸の周りの回転角が変動し、その分散は大きくなる。従って、車両１のヨー軸の周りの回転角の分散を計算することにより、車両１が旋回中であるか否かを判断することができる。

図２９は、図２６の倉庫１１０において車両１が移動するときのヨー軸の周りの回転角の標準偏差を示すグラフである。図２９の領域１１４ａ～１１４ｃは、図２６の対応する区間１１４ａ～１１４ｃを示す。図２６の区間１１４ａ，１１４ｂは通路１１１のコーナーを含み、ここで、車両１は旋回する。一方、図２６の区間１１４ｃでは、倉庫２２０の床面は凹凸を有し、これにより、車両１が旋回しなくても、ヨー軸の周りの回転角の分散が増大する。従って、車両１の旋回による分散の増大と、他の原因による分散の増大とを区別する必要がある。図２９の例では、例えば閾値０．０４を用いて、回転角［ｒａｄ］の標準偏差が０．０４より大きいとき、車両１が旋回中であると判断し、回転角の標準偏差が０．０４以下であるとき、車両１が旋回中ではないと判断することができる。

また、補正器３６は、車両１のヨー軸の周りの回転角に基づいて、例えば、所定時間だけ離れた第１及び第２の時刻にそれぞれ計算された回転角の差分に基づいて、車両１が旋回中であるか否かを判断してもよい。

また、補正器３６は、撮影装置１１の視野にマーカー４が含まれ、マーカー４に基づいて車両１の絶対姿勢が計算されているとき、絶対姿勢の変化に基づいて、車両１が旋回中であるか否かを判断してもよい。補正器３６は、絶対姿勢の変化に基づいて、車両１のヨー軸の周りの回転角又はその分散を計算し、回転角又はその分散の計算値が予め決められた閾値よりも大きいとき、車両１が旋回中であると判断してもよい。

さらに、補正器３６は、相対姿勢の変化及び絶対姿勢の変化の組み合わせに基づいて、車両１が旋回中であるか否かを判断してもよい。

また、補正器３６は、所定時間だけ離れた第１及び第２の時刻に撮影装置１１によってそれぞれ撮影された第１及び第２の画像の差分に基づいて、車両１が旋回中であるか否かを判断してもよい。この場合、補正器３６は、予め決められたランドマークを撮影し、ランドマークの画像に基づいて、車両１が旋回中であるか否かを判断してもよい。例えば、車両１が直進しているときと、車両１が旋回中であるときでは、所定時間だけ離れた時刻におけるランドマークの画像の変化（例えば、オプティカルフロー等の変化）は異なる。従って、ランドマークの画像に対して機械学習を行い、補正器３６は、学習結果のデータに基づいて車両の直進及び旋回を区別してもよい。

また、補正器３６は、記憶装置３５に格納されたマップ情報に含まれる通路１０１の形状と、車両１の現在の位置とに基づいて、車両１が旋回中であるか否かを判断してもよい。言い換えると、補正器３６は、マップ情報に基づいて、確実に旋回すると考えられる通路１０１の場所（例えばコーナー）を車両１が移動しているか、それとも、確実に旋回しないと考えられる通路１０１の場所（例えば直線区間）を判断し、これにより、車両１が旋回中であるか否かを判断してもよい。車両１の現在の位置は、計算された相対位置又は絶対位置からわかる。

図２５のステップＳ４４において、補正器３６は、車両１の相対姿勢及び絶対姿勢に基づいて、車両１の補正回転量を計算する。図２５のステップＳ４４は、第１の実施形態に係る図１４のステップＳ３３と同様である。

ステップＳ４５において、補正器３６は、車両１の補正回転量を車両１の相対姿勢に加算する。ここで、補正器３６は、ステップＳ４３がＮＯであった場合、ステップＳ４４において計算された補正回転量を用いて、補正された姿勢を計算する。一方、補正器３６は、ステップＳ４３がＹＥＳであった場合、補正回転量の計算を行わずに補正処理（Ｓ４Ａ）を終了する。これにより、補正器３６は、車両１が旋回中である期間に計算された絶対姿勢を使用せず、車両１が旋回中ではない期間に計算された絶対姿勢のみを使用して、補正された姿勢を生成する。なお、ステップＳ４３がＹＥＳであった場合、旋回中でないときに計算されて保存された補正回転量を相対姿勢に加算（ステップＳ４５）してもよい。

図３０は、第２の実施形態に係る測位装置１２を備えた車両１が通路１１１のコーナーを通過するときに計算される車両１の軌跡を示す図である。図３０の例では、車両１が通路１１１のコーナーを通過しているときに、測位装置１２は、マーカー４を検出し、マーカー４に基づいて絶対姿勢を計算する。車両１が旋回中である期間に計算された絶対姿勢は誤差を含む。従って、車両１が旋回中である期間に計算された絶対姿勢を用いて補正された姿勢を計算すると（旋回中の姿勢補正：あり）、車両１の位置及び軌跡は、実際の位置及び軌跡から大きくずれてしまう。一方、車両１が旋回中である期間に計算された絶対姿勢を使用しないことにより（旋回中の姿勢補正：なし）、車両１の位置及び軌跡は、実際の位置及び軌跡により近くなる。

図３１は、第２の実施形態の比較例に係る補正処理を実行することによって計算された車両１の軌跡１１５を示す図である。図３２は、図２５の補正処理を実行することによって計算された車両１の軌跡１１６を示す図である。また、図３１及び図３２は、車両１が点Ｃを起点かつ終点として移動したときに計算された車両１の軌跡を示す。また、図３１及び図３２では、図示の簡単化のために、マーカー４を省略している。図３１は、図２５のステップＳ４３を実行せず、車両１が旋回中であるか否かにかかわらず、常に、絶対姿勢に基づいて相対姿勢を補正する場合を示す。図３１によれば、車両１が旋回中である期間に計算された絶対姿勢を用いて補正された姿勢を計算した結果、計算された車両１の軌跡１０３は蛇行し、また、通路１０１から大きくはみ出している。一方、図３２によれば、車両１が旋回中である期間に計算された絶対姿勢を使用しないことにより、誤差の影響を低減することができ、計算された車両１の軌跡１０４において、蛇行及びはみ出しは小さくなっていることがわかる。

車両１が旋回中であるとき、絶対位置及び絶対姿勢に比べて、相対位置及び相対姿勢の誤差が大きくなり、特に相対位置の誤差が大きくなる。本願の発明者が行った実験によれば、車両１の旋回中にマーカー４を検出できなくなった場合、旋回中に計算された相対姿勢を、「最後」に検出されたマーカー４に基づいて計算された絶対姿勢で補正しても、低精度の相対姿勢に起因して、車両１の位置及び姿勢の精度が悪化することがわかった。これにより、補正された姿勢を有する車両１の軌跡が通路１０１をはみ出てしまう。

第２の実施形態によれば、Visual-SLAMなどを用いて特徴点４１に基づいて計算された相対位置及び相対姿勢を、マーカー４に基づいて計算された絶対位置及び絶対姿勢に基づいて補正することにより、車両１の位置及び姿勢を高精度に測定することができる。また、第２の実施形態によれば、補正された姿勢を生成するとき、車両１が旋回中である期間に計算された絶対姿勢を使用しないことにより、絶対姿勢の誤差による影響を低減し、車両１の位置及び姿勢をより高精度に測定することができる。

また、本願の発明者が行った実験によれば、測位装置１２によって計算される車両１の位置及び姿勢は、相対姿勢及び絶対姿勢の誤差に比べて、相対位置及び絶対位置の誤差の影響を受けにくいことがわかった。従って、図２５の例では、補正器３６は、車両１が旋回中である期間及び車両１が旋回中ではない期間に計算された絶対位置を使用して、補正された位置を生成する（ステップＳ４１～Ｓ４２）。しかしながら、相対位置及び絶対位置の誤差の影響を受けにくくするために、補正器３６は、車両１が旋回中である期間に計算された絶対位置を使用せず、車両１が旋回中ではない期間に計算された絶対位置のみを使用して、補正された位置を生成してもよい。

［第２の実施形態の変形例］
図３３は、第２の実施形態の変形例に係る測位装置１２Ａの構成を示すブロック図である。測位装置１２Ａは、図３の補正器３６に代えて、補正器３６Ａを備える。補正器３６Ａは、車両１に搭載された角速度センサ１６によって生成された車両１の角速度を含むセンサデータを取得する。角速度センサ１６は、例えばジャイロセンサである。角速度センサ１６は、撮影装置１１の筐体の内部又は外部に一体的に固定されてもよく、撮影装置１１の付近に固定されてもよい。補正器３６Ａは、車両１の角速度に基づいて、車両１が旋回中であるか否かを判断する。補正器３６Ａは、車両１の角速度が予め決められた閾値（例えば、０．１ラジアン／秒）よりも大きいとき、車両１が旋回中であると判断してもよい。

前述のように、相対姿勢又は絶対姿勢の変化に基づいて（すなわち、撮影装置１１によって撮影された画像に基づいて）車両１が旋回中であるか否かを判断しようとする場合、例えば照明又は背景の変動などに起因して画像が変動し、相対姿勢又は絶対姿勢の計算に誤差が生じることがある。この誤差に起因して、車両１が旋回中であるか否かの判断に誤りが生じることがある。この場合、補正器３６Ａは、相対姿勢又は絶対姿勢の変化に代えて、車両１の角速度に基づいて、車両１が旋回中であるか否かを判断してもよい。これにより、補正器３６Ａは、照明又は背景の変動などに起因する画像の変動の影響を受けることなく、車両１が旋回中であるか否かを高精度かつ安定的に判断することができる。

また、補正器３６Ａは、相対姿勢又は絶対姿勢の変化を参照することに加えて、車両１の角速度を参照することで、車両１が旋回中であるか否かを判断してもよい。相対姿勢又は絶対姿勢の変化と、車両１の角速度とのうち、いずれか一方の精度が低いと考えられる場合、他方に基づいて、車両１が旋回中であるか否かを判断してもよい。例えば、車両１が凹凸のある路面を移動するとき、角速度センサ１６によって検出される車両１の角速度は、路面の凹凸に敏感に応答して変動することがある。この場合、車両１の角速度ではなく、相対姿勢又は絶対姿勢の変化に基づいて、車両１が旋回中であるか否かを判断してもよい。これにより、補正器３６Ａは、路面の凹凸の影響を受けることなく、車両１が旋回中であるか否かを高精度かつ安定的に判断することができる。

このように、相対姿勢又は絶対姿勢の変化を参照することに代えて、又は、相対姿勢又は絶対姿勢の変化を参照することに加えて、車両１の角速度を参照することにより、車両１が旋回中であるか否かを高精度かつ安定的に判断することができる。

また、補正器３６Ａは、車両１の駆動機構１５の状態に基づいて、車両１が旋回中であるか否かを判断してもよい。補正器３６は、駆動機構１５に含まれる操舵装置の状態を参照してもよい。車両１が有人の乗物である場合、補正器３６は、コンソール１ｃ（例えばハンドル）の状態を参照してもよい。

［第２の実施形態の効果等］
第２の実施形態によれば、測位装置１２，１２Ａは、第１の計算器、記憶装置３５、第２の計算器、及び補正器３６，３６Ａを備える。車両１に搭載された撮影装置１１によって撮影された複数の画像に基づいて、所定の基準位置及び基準姿勢に対する車両１の相対位置及び相対姿勢を示す車両１の第１の位置及び第１の姿勢を計算する。予め決められた位置に配置されて視覚的に識別可能な複数のマーカー４の識別子、位置、及び姿勢の情報と、車両１のための通路１０１を含むマップの情報とを格納する。撮影装置１１によって撮影された画像から複数のマーカー４のうちの１つを抽出し、抽出された１つのマーカー４の位置及び姿勢に基づいて、マップにおける車両１の位置及び姿勢を示す車両１の第２の位置及び第２の姿勢を計算する。第２の位置及び第２の姿勢に基づいて第１の位置及び第１の姿勢を補正し、車両１の補正された位置及び補正された姿勢を生成する。補正器３６，３６Ａは、車両１が旋回中であるか否かを判断し、車両１が旋回中である期間に計算された第２の姿勢を使用せず、車両１が旋回中ではない期間に計算された第２の姿勢のみを使用して、補正された姿勢を生成する。

これにより、Visual-SLAMなどを用いて特徴点４１に基づいて計算された相対位置及び相対姿勢を、マーカー４に基づいて計算された絶対位置及び絶対姿勢に基づいて補正することにより、車両１の位置及び姿勢を高精度に測定することができる。また、補正された姿勢を生成するとき、車両１が旋回中である期間に計算された絶対姿勢を使用しないことにより、絶対姿勢の誤差による影響を低減し、車両１の位置及び姿勢をより高精度に測定することができる。

第２の実施形態によれば、補正器３６，３６Ａは、車両１が旋回中である期間及び車両１が旋回中ではない期間に計算された第２の位置を使用して、補正された位置を生成してもよい。

これにより、補正器３６，３６Ａは、車両１の絶対位置を継続的に計算し、従って、車両１の位置を継続的に補正することができる。

第２の実施形態によれば、補正器３６は、第１の姿勢又は第２の姿勢の変化に基づいて、車両１が旋回中であるか否かを判断してもよい。

これにより、補正器３６は、撮影装置１１によって撮影された画像に基づいて、車両１が旋回中であるか否かを判断することができる。

第２の実施形態によれば、補正器３６は、第１の姿勢又は第２の姿勢の変化に基づいて車両１のヨー軸の周りの回転角又はその分散を計算し、回転角又はその分散の計算値が予め決められた閾値よりも大きいとき、車両１が旋回中であると判断してもよい。

これにより、補正器３６は、閾値よりも大きな回転角又はその分散で車両１が旋回中である期間に計算された絶対姿勢を使用せず、閾値よりも小さな回転角又はその分散で車両１が旋回中である期間に計算された絶対姿勢のみを使用して、補正された姿勢を生成することができる。

第２の実施形態によれば、補正器３６Ａは、車両１に搭載された角速度センサ１６によって生成された車両１の角速度を含むセンサデータを取得し、車両１の角速度に基づいて、車両１が旋回中であるか否かを判断してもよい。

これにより、補正器３６Ａは、角速度センサ１６によって生成された車両１の角速度に基づいて、車両１が旋回中であるか否かを判断することができる。

第２の実施形態によれば、補正器３６Ａは、車両１の角速度が予め決められた閾値よりも大きいとき、車両１が旋回中であると判断してもよい。

これにより、補正器３６Ａは、閾値よりも大きな回転角又はその分散で車両１が旋回中である期間に計算された絶対姿勢を使用せず、閾値よりも小さな回転角又はその分散で車両１が旋回中である期間に計算された絶対姿勢のみを使用して、補正された姿勢を生成することができる。

第２の実施形態によれば、補正器３６は、マップに含まれた通路１０１の形状と、車両１の現在の位置とに基づいて、車両１が旋回中であるか否かを判断してもよい。

第２の実施形態によれば、補正器３６は、車両１の操舵装置の状態に基づいて、車両１が旋回中であるか否かを判断してもよい。

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

また、第１の実施形態（通路１０１の向きに基づいて車両１の相対姿勢を補正すること）と、第２の実施形態（車両１が旋回中である期間に計算された絶対姿勢を使用しないこと）とを組み合わせてもよい。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Claims (17)

1. 移動体に搭載された撮影装置によって撮影された複数の画像に基づいて、所定の基準位置及び基準姿勢に対する前記移動体の相対位置及び相対姿勢を示す前記移動体の第１の位置及び第１の姿勢を計算する第１の計算器と、
   予め決められた位置に配置されて視覚的に識別可能な複数のマーカーの識別子、位置、及び姿勢の情報と、前記移動体のための通路を含むマップの情報とを格納する記憶装置と、
   前記撮影装置によって撮影された画像から前記複数のマーカーのうちの１つを抽出し、抽出された前記１つのマーカーの位置及び姿勢に基づいて、前記マップにおける前記移動体の位置及び姿勢を示す前記移動体の第２の位置及び第２の姿勢を計算する第２の計算器と、
   前記第２の位置及び前記第２の姿勢に基づいて前記第１の位置及び前記第１の姿勢を補正し、前記移動体の補正された位置及び補正された姿勢を生成する補正器とを備え、
   前記補正器は、抽出された前記１つのマーカーが通路に沿って配置され、かつ、前記補正された位置及び前記補正された姿勢を有する前記移動体の進行方向と、前記通路の向きとの角度差が予め決められた第１の閾値よりも小さいとき、前記第２の姿勢に代えて前記通路の向きに基づいて前記第１の姿勢を補正する、
   測位装置。
2. 前記補正器は、前記第２の姿勢の向きと前記通路の向きとの角度差が前記第１の閾値よりも小さいとき、前記第２の姿勢に代えて前記通路の向きに基づいて前記第１の姿勢を補正する、
   請求項１記載の測位装置。
3. 前記補正器は、前記補正された位置を有する移動体の直近の移動を示すベクトルの向きと、前記通路の向きとの角度差が前記第１の閾値よりも小さいとき、前記第２の姿勢に代えて前記通路の向きに基づいて前記第１の姿勢を補正する、
   請求項１記載の測位装置。
4. 前記補正器は、
   予め決められた時間長にわたって、又は、予め決められた個数の連続した画像に対応して、前記補正された位置を有する移動体の直近の移動を示すベクトルの向きを計算し、
   複数の前記ベクトルの向きの平均値と、前記通路の向きとの角度差が前記第１の閾値よりも小さいとき、前記第２の姿勢に代えて前記通路の向きに基づいて前記第１の姿勢を補正する、
   請求項３記載の測位装置。
5. 前記補正器は、予め決められた時間長にわたって、又は、予め決められた個数の連続した画像に対応してそれぞれ計算された複数の前記第１の姿勢の平均値を、前記第２の姿勢又は前記通路の向きに基づいて補正する、
   請求項１～４のうちの１つに記載の測位装置。
6. 前記補正器は、前記移動体が旋回中であるか否かを判断し、前記移動体が旋回中である期間に計算された前記第２の姿勢を使用せず、前記移動体が旋回中ではない期間に計算された前記第２の姿勢のみを使用して、前記補正された姿勢を生成する、
   請求項１～５のうちの１つに記載の測位装置。
7. 前記補正器は、前記第１の姿勢又は前記第２の姿勢の変化に基づいて、前記移動体が旋回中であるか否かを判断する、
   請求項６に記載の測位装置。
8. 前記補正器は、前記第１の姿勢又は前記第２の姿勢の変化に基づいて前記移動体のヨー軸の周りの回転角又はその分散を計算し、前記回転角又はその分散の計算値が予め決められた第２の閾値よりも大きいとき、前記移動体が旋回中であると判断する、
   請求項７記載の測位装置。
9. 前記補正器は、前記移動体に搭載された角速度センサによって生成された前記移動体の角速度を含むセンサデータを取得し、前記移動体の角速度に基づいて、前記移動体が旋回中であるか否かを判断する、
   請求項６に記載の測位装置。
10. 前記補正器は、前記移動体の角速度が予め決められた第３の閾値よりも大きいとき、前記移動体が旋回中であると判断する、
    請求項９に記載の測位装置。
11. 前記移動体に搭載された１つ又は複数のセンサによって生成されたセンサデータを取得し、前記センサデータを、前記補正された位置及び前記補正された姿勢のデータに合成するデータ合成器をさらに備えた、
    請求項１～１０のうちの１つに記載の測位装置。
12. 前記データ合成器は、前記移動体の加速度及び角速度の少なくとも一方を含むセンサデータを取得する、
    請求項１１記載の測位装置。
13. 前記データ合成器は、前記移動体によって搬送される荷物の重さを含むセンサデータを取得する、
    請求項１１又は１２記載の測位装置。
14. 前記測位装置は、前記撮影装置によって撮影された画像から予め決められた対象物を認識する画像認識器をさらに備え、
    前記データ合成器は、前記画像認識器によって認識された対象物の情報を、前記補正された位置及び前記補正された姿勢のデータに合成する、
    請求項１１～１３のうちの１つに記載の測位装置。
15. 前記画像認識器は人物を認識する、
    請求項１４記載の測位装置。
16. 前記画像認識器は、予め学習された特定の荷物を認識する、
    請求項１４記載の測位装置。
17. 撮影装置と、
    請求項１～１６のうちの１つに記載の測位装置とを備えた、
    移動体。

Images