特許 7599136 測位装置及び移動体

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-12-05

(45)【発行日】2024-12-13

(54)【発明の名称】測位装置及び移動体

(51)【国際特許分類】

   G01C 15/00 20060101AFI20241206BHJP

   G05D 1/43 20240101ALI20241206BHJP

【ＦＩ】

G01C15/00 101

G05D1/43

【請求項の数】 15

(21)【出願番号】P 2021575605

(86)(22)【出願日】2020-09-24

(86)【国際出願番号】 JP2020036114

(87)【国際公開番号】W WO2021157123

(87)【国際公開日】2021-08-12

【審査請求日】2023-08-04

(31)【優先権主張番号】P 2020019935

(32)【優先日】2020-02-07

(33)【優先権主張国・地域又は機関】JP

(73)【特許権者】

【識別番号】314012076

【氏名又は名称】パナソニックＩＰマネジメント株式会社

(74)【代理人】

【識別番号】100132241

【弁理士】

【氏名又は名称】岡部 博史

(74)【代理人】

【識別番号】100135703

【弁理士】

【氏名又は名称】岡部 英隆

(74)【代理人】

【識別番号】100199314

【弁理士】

【氏名又は名称】竹内 寛

(72)【発明者】

【氏名】岡田 典

(72)【発明者】

【氏名】桂 右京

(72)【発明者】

【氏名】石上 智英

【審査官】櫻井 仁

(56)【参考文献】

【文献】国際公開第２０１９／１３０９３１（ＷＯ，Ａ１）

【文献】特開平１０－０６９２１９（ＪＰ，Ａ）

【文献】国際公開第２０１９／１３８８３６（ＷＯ，Ａ１）

【文献】国際公開第２０２０／１３７３１２（ＷＯ，Ａ１）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０１Ｃ １５／００

Ｇ０５Ｄ １／４３

Ｇ０１Ｂ １１／００

Ｇ０１Ｂ ２１／００

(57)【特許請求の範囲】

【請求項1】

移動体に搭載された撮影装置によって撮影された複数の画像に基づいて、所定の基準位置及び基準姿勢に対する前記移動体の相対位置及び相対姿勢を示す前記移動体の第１の位置及び第１の姿勢を計算する第１の計算器と、
予め決められた位置に配置されて視覚的に識別可能な複数のマーカーの識別子、位置、及び姿勢の情報と、前記移動体のための通路を含むマップの情報とを格納する記憶装置と、
前記撮影装置によって撮影された画像から前記複数のマーカーのうちの１つを抽出し、抽出された前記１つのマーカーの位置及び姿勢に基づいて、前記マップにおける前記移動体の位置及び姿勢を示す前記移動体の第２の位置及び第２の姿勢を計算する第２の計算器と、
前記第１の計算器によって計算された前記第１の位置及び前記第１の姿勢の信頼度を示す第１の信頼度と、前記第２の計算器によって計算された前記第２の位置及び前記第２の姿勢の信頼度を示す第２の信頼度とを計算する信頼度計算器と、
前記第１の信頼度が前記第２の信頼度以上であるとき、前記第１の位置及び前記第１の姿勢を前記移動体の位置及び姿勢として決定し、前記第１の信頼度が前記第２の信頼度より小さいとき、前記第２の位置及び前記第２の姿勢を前記移動体の位置及び姿勢として決定する位置及び姿勢決定器とを備えた、
測位装置。

【請求項2】

前記信頼度計算器は、前記移動体の基準位置及び基準姿勢からの移動距離及び回転角度が小さくなるほど前記第１の信頼度が増大するように前記第１の信頼度を計算し、
前記位置及び姿勢決定器が前記第２の位置及び前記第２の姿勢を前記移動体の位置及び姿勢として決定したとき、前記信頼度計算器は前記基準位置及び前記基準姿勢をリセットする、
請求項１記載の測位装置。

【請求項3】

前記信頼度計算器は、前記第２の位置及び前記第２の姿勢に対する前記第１の位置及び前記第１の姿勢の差が小さくなるほど前記第１の信頼度が増大するように前記第１の信頼度を計算する、
請求項１記載の測位装置。

【請求項4】

前記第１の計算器は、時間的に隣接した複数の画像においてトラッキングされた特徴点に基づいて前記第１の位置及び前記第１の姿勢を計算し、
前記信頼度計算器は、時間的に隣接した複数の画像においてトラッキングに成功した特徴点の個数に基づいてトラッキングの信頼度を計算し、前記トラッキングの信頼度が高くなるほど前記第１の信頼度が増大するように前記第１の信頼度を計算する、
請求項２又は３記載の測位装置。

【請求項5】

前記信頼度計算器は、前記第１の位置又は前記第１の姿勢の変化量の分散が小さくなるほど前記第１の信頼度が増大するように前記第１の信頼度を計算する、
請求項１記載の測位装置。

【請求項6】

前記信頼度計算器は、前記撮影装置によって撮影された画像において、前記画像の中心から前記マーカーまでの距離が小さくなるほど前記第２の信頼度が増大するように前記第２の信頼度を計算する、
請求項１～５のうちの１つに記載の測位装置。

【請求項7】

前記信頼度計算器は、前記撮影装置によって撮影された画像における前記マーカーの見かけのサイズが大きくなるほど前記第２の信頼度が増大するように前記第２の信頼度を計算する、
請求項１～６のうちの１つに記載の測位装置。

【請求項8】

前記位置及び姿勢決定器は、前記撮影装置によって撮影された画像における前記マーカーの見かけのサイズが予め決められた閾値よりも小さいとき、前記第１の位置及び前記第１の姿勢を前記移動体の位置及び姿勢として決定する、
請求項１～７のうちの１つに記載の測位装置。

【請求項9】

前記移動体に搭載された１つ又は複数のセンサによって生成されたセンサデータを取得し、前記センサデータを、前記位置及び姿勢決定器によって決定された前記移動体の位置及び姿勢のデータに合成するデータ合成器をさらに備えた、
請求項１～８のうちの１つに記載の測位装置。

【請求項10】

前記データ合成器は、前記移動体の加速度及び角速度の少なくとも一方を含むセンサデータを取得する、
請求項９記載の測位装置。

【請求項11】

前記データ合成器は、前記移動体によって搬送される荷物の重さを含むセンサデータを取得する、
請求項９又は１０記載の測位装置。

【請求項12】

前記測位装置は、前記撮影装置によって撮影された画像から予め決められた対象物を認識する画像認識器をさらに備え、
前記データ合成器は、前記画像認識器によって認識された対象物の情報を、前記位置及び姿勢決定器によって決定された前記移動体の位置及び姿勢のデータに合成する、
請求項９～１１のうちの１つに記載の測位装置。

【請求項13】

前記画像認識器は人物を認識する、
請求項１２記載の測位装置。

【請求項14】

前記画像認識器は、予め学習された特定の荷物を認識する、
請求項１２記載の測位装置。

【請求項15】

撮影装置と、
請求項１～１４のうちの１つに記載の測位装置とを備えた、
移動体。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

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

【0003】

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

【0004】

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

【先行技術文献】

【特許文献】

【0005】

【文献】特開２０１１－２１９２２９号公報

【非特許文献】

【0006】

【文献】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

【発明の概要】

【発明が解決しようとする課題】

【0007】

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

【0008】

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

【課題を解決するための手段】

【0009】

本開示の一態様によれば、
移動体に搭載された撮影装置によって撮影された複数の画像に基づいて、所定の基準位置及び基準姿勢に対する前記移動体の相対位置及び相対姿勢を示す前記移動体の第１の位置及び第１の姿勢を計算する第１の計算器と、
予め決められた位置に配置されて視覚的に識別可能な複数のマーカーの識別子、位置、及び姿勢の情報と、前記移動体のための通路を含むマップの情報とを格納する記憶装置と、
前記撮影装置によって撮影された画像から前記複数のマーカーのうちの１つを抽出し、抽出された前記１つのマーカーの位置及び姿勢に基づいて、前記マップにおける前記移動体の位置及び姿勢を示す前記移動体の第２の位置及び第２の姿勢を計算する第２の計算器と、
前記第１の計算器によって計算された前記第１の位置及び前記第１の姿勢の信頼度を示す第１の信頼度と、前記第２の計算器によって計算された前記第２の位置及び前記第２の姿勢の信頼度を示す第２の信頼度とを計算する信頼度計算器と、
前記第１の信頼度が前記第２の信頼度以上であるとき、前記第１の位置及び前記第１の姿勢を前記移動体の位置及び姿勢として決定し、前記第１の信頼度が前記第２の信頼度より小さいとき、前記第２の位置及び前記第２の姿勢を前記移動体の位置及び姿勢として決定する位置及び姿勢決定器とを備える。

【0010】

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

【発明の効果】

【0011】

本開示の一態様によれば、第１の計算器によって計算された相対位置及び相対姿勢の信頼度と、第２の計算器によって計算された絶対位置及び絶対姿勢の信頼度とを計算し、信頼度が高いほうを移動体の位置及び姿勢として決定することにより、移動体の位置及び姿勢を高精度に測定することができる。

【図面の簡単な説明】

【0012】

【図1】第１の実施形態に係る車両１の構成を示す概略図である。

【図2】図１の車両１を含む測位システムの構成を示すブロック図である。

【図3】図２の測位装置１２の構成を示すブロック図である。

【図4】図１の車両１が移動する通路１０１を含む倉庫１００のマップである。

【図5】図４のマーカー４の例を示す図であって、（ａ）はマーカー４の正面図を示し、（ｂ）はマーカー４の上面図を示す。

【図6】図３の記憶装置３５に格納されたマーカー情報の例を示すテーブルである。

【図7】図１の撮影装置１１によって撮影された画像４０の例を示す図である。

【図8】図２の測位装置１２によって実行される測位処理を示すフローチャートである。

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

【図10】図３の画像処理器３１によって抽出される特徴点であって、（ａ）は時刻ｎにおける画像４０（ｎ）から抽出される特徴点Ｆ１，Ｆ２を示し、（ｂ）は時刻ｎ’における画像４０（ｎ’）から抽出される特徴点Ｆ１’，Ｆ２’を示す図である。

【図11】図３の相対位置計算器３２によって計算される相対位置及び相対姿勢の信頼度の計算を説明する図である。

【図12】図８のステップＳ４（絶対位置計算処理）のサブルーチンを示すフローチャートである。

【図13】マーカー座標系におけるマーカー４の頂点の座標を示す図である。

【図14】図１の撮影装置１１によって撮影された画像４０におけるマーカー４の頂点の座標を示す図である。

【図15】図３の絶対位置計算器３４によって計算される絶対位置及び絶対姿勢の信頼度の計算を説明する図である。

【図16】絶対位置の誤差が、図１の撮影装置１１によって撮影された画像におけるマーカー４の見かけのサイズに依存することを説明するグラフである。

【図17】絶対姿勢の誤差が、図１の撮影装置１１によって撮影された画像におけるマーカー４の見かけのサイズに依存することを説明するグラフである。

【図18】第１の実施形態の第１の変形例に係る測位処理における相対位置及び相対姿勢の信頼度の計算を説明する図である。

【図19】第１の実施形態の第２の変形例に係る測位処理における相対位置及び相対姿勢の信頼度の計算を説明する図である。

【図20】第１の実施形態の第３の変形例に係る測位処理における相対位置及び相対姿勢の信頼度の計算を説明する図であって、車両１の第１の軌跡を示す図である。

【図21】図２０の車両１の相対位置又は相対姿勢の変化量を概略的に示すグラフである。

【図22】第１の実施形態の第３の変形例に係る測位処理における相対位置及び相対姿勢の信頼度の計算を説明する図であって、車両１の第２の軌跡を示す図である。

【図23】図２２の車両１の相対位置又は相対姿勢の変化量を概略的に示すグラフである。

【図24】第１の実施形態の第４の変形例に係る測位処理における絶対位置及び絶対姿勢の信頼度の計算を説明する図である。

【図25】図１の撮影装置１１によって撮影されるマーカー４ａ－１～４ａ－１２の例示的な配置を示す図である。

【図26】図２５のマーカー４ａ－１～４ａ－１２を撮影装置１１によって撮影した画像４０の例を示す図である。

【図27】図１の撮影装置１１によって撮影されるマーカー４ｂ－１～４ｂ－１２の例示的な配置を示す図である。

【図28】図２７のマーカー４ｂ－１～４ｂ－１２を撮影装置１１によって撮影した画像４０の例を示す図である。

【図29】図１の撮影装置１１の光軸に対するマーカー４の面の角度を誤検出したことに起因する、マーカー座標系における撮影装置１１の位置の変動を説明する図である。

【図30】第１の実施形態の第５の変形例に係る測位処理における絶対位置及び絶対姿勢の信頼度の計算を説明する図である。

【図31】第２の実施形態に係る測位装置１２Ｂの構成を示すブロック図である。

【発明を実施するための形態】

【0013】

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

【0014】

［第１の実施形態］
以下、第１の実施形態に係る測位装置及びそれを備えた移動体について説明する。

【0015】

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

【0016】

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

【0017】

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

【0018】

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

【0019】

測位装置１２は、撮影装置１１によって撮影された画像に基づいて、車両１の位置及び姿勢を計算する。測位装置１２は、撮影装置１１によって撮影された複数の画像から特徴点を抽出し、抽出された特徴点を画像間で対応付けし、画像間の特徴点の変化量に基づいて、所定の基準位置及び基準姿勢に対する車両１の相対位置及び相対姿勢を計算する。また、測位装置１２は、撮影装置１１によって撮影された画像から、予め決められた位置に配置されて視覚的に識別可能な複数のマーカーのうちの１つを抽出し、抽出された１つのマーカーに基づいて、予め与えられたマップにおける車両１の絶対位置及び絶対姿勢を計算する。測位装置１２はさらに、相対位置及び相対姿勢の信頼度と、絶対位置及び絶対姿勢の信頼度とを計算し、信頼度が高いほうを車両１の位置及び姿勢として決定する。信頼度は、例えば、予め実験的に決定された理論誤差（すなわち、位置及び姿勢の真値からの統計的な誤差）のモデルを参照し、撮影装置１１によって撮影された画像に基づいて計算された推定誤差によって表される。信頼度は推定誤差が小さいほど大きくなる。

【0020】

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

【0021】

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

【0022】

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

【0023】

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

【0024】

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

【0025】

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

【0026】

［測位装置１２の構成］
図３は、図２の測位装置１２の構成を示すブロック図である。測位装置１２は、画像処理器３１、相対位置計算器３２、画像認識器３３、絶対位置計算器３４、記憶装置３５、信頼度計算器３６、ならびに位置及び姿勢決定器３７を備える。

【0027】

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

【0028】

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

【0029】

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

【0030】

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

【0031】

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

【0032】

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

【0033】

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

【0034】

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

【0035】

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

【0036】

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

【0037】

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

【0038】

信頼度計算器３６は、相対位置計算器３２によって計算された相対位置及び相対姿勢の信頼度と、絶対位置計算器３４によって計算された絶対位置及び絶対姿勢の信頼度とを計算する。信頼度計算器３６は、例えば、車両１の基準位置及び基準姿勢からの移動距離及び回転角度が小さくなるほど信頼度が増大するように、また、移動距離及び回転角度が大きくなるほど信頼度が低下するように、相対位置及び相対姿勢の信頼度を計算する。例えば、後述するように位置及び姿勢決定器３７が絶対位置及び絶対姿勢を車両１の位置及び姿勢として決定したとき、車両１の基準位置及び基準姿勢はリセットされる。言いかえると、絶対位置及び絶対姿勢の信頼度が相対位置及び相対姿勢の信頼度を越えたときの絶対位置及び絶対姿勢が、車両１の基準位置及び基準姿勢として設定される。また、信頼度計算器３６は、例えば、撮影装置１１によって撮影された画像におけるマーカー４の見かけのサイズが大きくなるほど信頼度が増大するように、また、画像におけるマーカー４の見かけのサイズが小さくなるほど信頼度が低下するように、絶対位置及び絶対姿勢の信頼度を計算する。

【0039】

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

【0040】

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

【0041】

［第１の実施形態の動作］
測位装置１２によれば、Visual-SLAMを用いて計算された相対位置及び相対姿勢の誤差が増大したとき、マーカー４に基づいて計算された絶対位置及び絶対姿勢により、車両１の位置及び姿勢を補正することができる。

【0042】

しかしながら、Visual-SLAMを用いて車両１の相対位置及び相対姿勢を計算する場合、車両１の基準位置及び基準姿勢からの移動距離及び回転角度が増大するほど、相対位置及び相対姿勢の誤差が増大する。また、マーカー４に基づいて車両１の絶対位置及び絶対姿勢を計算する場合、撮影装置１１によってマーカー４を撮影する条件に応じて、絶対位置及び絶対姿勢の誤差が増大する。例えば、撮影装置１１からマーカー４までの距離が増大するほど、絶対位置及び絶対姿勢の誤差が増大する。また、撮影装置１１の光軸に対するマーカー４の面の角度が０度に近いとき、絶対位置及び絶対姿勢の誤差が増大する。また、撮影された画像の端部にマーカー４が位置しているとき、絶対位置及び絶対姿勢の誤差が増大することがある。

【0043】

絶対位置及び絶対姿勢の誤差が大きい場合には、絶対位置及び絶対姿勢を車両１の位置及び姿勢として使用しないことが望ましい。一方、相対位置及び相対姿勢の誤差が蓄積しすぎた場合には、ある程度、絶対位置及び絶対姿勢の誤差が大きくても、絶対位置及び絶対姿勢を車両１の位置及び姿勢として決定することが望ましい。この場合、相対位置及び相対姿勢の誤差の大きさと、絶対位置及び絶対姿勢の誤差の大きさとを評価する必要がある。

【0044】

従って、実施形態に係る測位装置１２は、相対位置及び相対姿勢の信頼度と、絶対位置及び絶対姿勢の信頼度とを計算し、信頼度が高いほうを車両１の位置及び姿勢として決定する。

【0045】

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

【0046】

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

【0047】

ステップＳ１において、測位装置１２は、撮影装置１１によって撮影された画像を取得する。

【0048】

ステップＳ２において、画像処理器３１及び相対位置計算器３２は、相対位置計算処理を実行し、車両１の相対位置及び相対姿勢を計算する。ステップＳ３において、信頼度計算器３６は、車両１の相対位置及び相対姿勢の信頼度Ｑ１を計算する。

【0049】

ステップＳ４において、画像認識器３３及び絶対位置計算器３４は、絶対位置計算処理を実行し、車両１の絶対位置及び絶対姿勢を計算する。ステップＳ５において、信頼度計算器３６は、車両１の絶対位置及び絶対姿勢の信頼度Ｑ２を計算する。

【0050】

ステップＳ２～Ｓ５は、図８に示すように並列に実行されてもよく、逐次に実行されてもよい。

【0051】

ステップＳ６において、信頼度計算器３６は、「絶対位置の信頼度＞相対位置の信頼度」と、「絶対姿勢の信頼度＞相対姿勢の信頼度」とのうちの少なくとも一方が成り立つか否かを判断し、ＹＥＳのときはステップＳ７に進み、ＮＯのときはステップＳ９に進む。信頼度計算器３６に代わって位置及び姿勢決定器３７がステップＳ６を実行してもよい。

【0052】

ステップＳ７において、位置及び姿勢決定器３７は、車両１の絶対位置及び絶対姿勢を、車両１の位置及び姿勢として決定する。位置及び姿勢決定器３７は、決定された車両１の位置及び姿勢を、通信装置１３及び表示装置１４に出力する。ステップＳ８において、位置及び姿勢決定器３７は、車両１の相対位置及び相対姿勢の基準をリセットする。

【0053】

ステップＳ９において、位置及び姿勢決定器３７は、車両１の相対位置及び装置姿勢を、車両１の位置及び姿勢として決定する。位置及び姿勢決定器３７は、決定された車両１の位置及び姿勢を、通信装置１３及び表示装置１４に出力する。

【0054】

ステップＳ８，Ｓ９の後でステップＳ１に戻り、処理を繰り返す。

【0055】

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

【0056】

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

【0057】

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

【0058】

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

【0059】

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

【0060】

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

【0061】

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

【0062】

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

【0063】

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

【0064】

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

【0065】

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

【0066】

【数1】

【0067】

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

【0068】

【数2】

【0069】

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

【0070】

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

【0071】

［相対位置及び相対姿勢の信頼度］
図１１は、図３の相対位置計算器３２によって計算される相対位置及び相対姿勢の信頼度の計算を説明する図である。

【0072】

信頼度計算器３６は、例えば、車両１の基準位置及び基準姿勢からの移動距離及び回転角度が小さくなるほど信頼度が増大するように、また、移動距離及び回転角度が大きくなるほど信頼度が低下するように、相対位置及び相対姿勢の信頼度を計算する。相対位置及び相対姿勢の信頼度は、例えば、予め実験的に決定された理論誤差（すなわち、位置及び姿勢の真値からの統計的な誤差）のモデルを参照し、撮影装置１１によって撮影された画像に基づいて計算された推定誤差によって表される。信頼度は推定誤差が小さいほど大きくなる。

【0073】

事前実験として、さまざまな条件で車両１を実際に走行させ、実際の経路における車両１の位置及び姿勢と、Visual-SLAMを用いて推定された経路における車両１の位置及び姿勢との差を計算する。これにより、Visual-SLAMの理論誤差、すなわち、車両１の移動距離の理論誤差Ｅｒａと、車両１の移動中の回転角度の理論誤差Ｅｒｂとを取得する。ここで、移動距離の理論誤差Ｅｒａは、例えば、さまざまな条件で計算して平均された、実際の移動距離に対する、Visual-SLAMを用いて推定される移動距離の誤差の比を示す。回転角度の理論誤差Ｅｒｂは、例えば、さまざまな条件で計算して平均された、実際の回転角度に対する、Visual-SLAMを用いて推定される回転角度の誤差の比Ｅｒｂを示す。

【0074】

図１１の例では、車両１は、点Ｐ０から点Ｐ１に至る経路を実際に走行する。実際の経路Ｐ０→Ｐ１は、例えば、長さｄ１＝５０ｍを有し、９０度のコーナーを２つ含む。従って、車両１は、経路Ｐ０→Ｐ１を進む間に、Ａ１＝｜９０度｜×２＝１８０度にわたって回転する。また、車両１が点Ｐ１に到着したとき、Visual-SLAMを用いて推定された車両１の位置は点Ｐ２にある。点Ｐ１，Ｐ２は、車両１の進行方向に沿った位置の誤差Ｅ１ａａ＝３．８５ｍと、車両１の進行方向に直交する方向に沿った位置の誤差Ｅ１ａｂ＝１．６ｍとを有する。従って、Visual-SLAMを用いて推定された経路Ｐ０→Ｐ２は、長さｄ２＝（（ｄ１－Ｅ１ａａ）^２＋Ｅ１ａｂ^２）^１／２＝（（５０ｍ－３．８５ｍ）^２＋１．６^２）^１／２＝４６．２ｍを有する。また、経路Ｐ０→Ｐ１及び経路Ｐ０→Ｐ２は、角度の誤差Ｅ１ｂ＝ａｒｃｓｉｎ（Ｅ１ａｂ／ｄ２）＝ａｒｃｓｉｎ（１．６／４６．２）＝２度を有する。この場合、Visual-SLAMの理論誤差、すなわち、移動距離の理論誤差Ｅｒａ及び回転角度の理論誤差Ｅｒｂは、以下のように計算される。

【0075】

Ｅｒａ＝Ｅ１ａａ／ｄ１＝３．８５／５０＝０．０７７
Ｅｒｂ＝Ｅ１ｂ／Ａ１＝２／１８０＝０．０１１

【0076】

ここで、移動距離の理論誤差Ｅｒａは、車両１の進行方向に沿った移動距離の成分に基づいて計算される。

【0077】

このように予め実験的に決定された理論誤差のモデルが与えられ、車両１が任意の経路に沿って移動するとき、車両１の相対位置及び相対姿勢の推定誤差は以下のように計算される。

【0078】

Visual-SLAMの理論誤差として、Ｅｒａ＝０．０７７及びＥｒｂ＝０．０１１が与えられ、車両１が、長さｄ１＝５０ｍを有し、９０度のコーナーを２つ含む経路Ｐ０→Ｐ１を走行する場合を考える。実際の経路Ｐ０→Ｐ１の長さｄ１＝５０ｍ及び回転角度Ａ１＝１８０度は未知である。一方、Visual-SLAMを用いて推定された経路Ｐ０→Ｐ２は、例えば、長さｄ２＝４６．２ｍ及び回転角度Ａ２＝１８２度を有することがわかる。従って、相対位置の推定誤差Ｅ１ａは以下のように計算される。

【0079】

Ｅ１ｂ＝Ａ２×Ｅｒｂ＝１８２度×０．０１１＝２度
Ｅ１ａｂ＝ｄ２×ｓｉｎ（Ｅ１ｂ）＝４６．２ｍ×ｓｉｎ（２度）＝１．６ｍ
Ｅ１ａａ＝ｄ２×ｃｏｓ（Ｅ１ｂ）×Ｅｒａ／（１±Ｅｒａ）＝４６．２ｍ×ｃｏｓ（２度）×０．０７７／（１±０．０７７）＝３．３ｍ，３．８５ｍ
Ｅ１ａ＝（Ｅ１ａａ^２＋Ｅ１ａｂ^２）^１／２＝（３．８５^２＋１．６^２）^１／２＝４．２ｍ

【0080】

一方、相対姿勢の推定誤差は、経路Ｐ０→Ｐ１及び経路Ｐ０→Ｐ２の間の角度の推定誤差Ｅ１ｂに等しい。

【0081】

車両１の基準位置及び基準姿勢からの移動距離及び回転角度が大きくなるほど、相対位置及び相対姿勢の推定誤差は増大する（すなわち、相対位置及び相対姿勢の信頼度が低下する）。また、移動距離及び回転角度が小さくなるほど、相対位置及び相対姿勢の推定誤差は低下する（すなわち、相対位置及び相対姿勢の信頼度が増大する）。従って、相対位置及び相対姿勢の推定誤差を用いて、相対位置及び相対姿勢の信頼度を表すことができる。

【0082】

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

【0083】

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

【0084】

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

【0085】

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

【0086】

図１２のステップＳ２２において、絶対位置計算器３４は、撮影装置１１によって撮影された画像におけるマーカー４の見かけのサイズを取得する。世界座標系において車両１が主に水平面内（すなわち、Ｘｗ－Ｙｗ面内）において移動する場合、画像におけるマーカー４の見かけのサイズは、例えば、マーカー４の縦方向（すなわちＹｉ方向）の長さ（画素数）によって表されてもよい。それに代わって、画像におけるマーカー４の見かけのサイズは、マーカー４の縦方向の長さ及び横方向の長さの組み合わせによって表されてもよい。画像におけるマーカー４の見かけのサイズは、撮影装置１１からマーカー４までの距離に対応する。なお、マーカー４の横方向（すなわちＸｉ方向）の長さは、距離に依存するだけでなく、撮影装置１１によりマーカー４を撮影する角度にも依存して変化する。

【0087】

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

【0088】

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

【0089】

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

【0090】

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

【0091】

［絶対位置及び絶対姿勢の信頼度］
図１５は、図３の絶対位置計算器３４によって計算される絶対位置及び絶対姿勢の信頼度の計算を説明する図である。

【0092】

信頼度計算器３６は、例えば、撮影装置１１によって撮影された画像におけるマーカー４の見かけのサイズが大きくなるほど信頼度が増大するように、また、画像におけるマーカー４の見かけのサイズが小さくなるほど信頼度が低下するように、絶対位置及び絶対姿勢の信頼度を計算する。絶対位置及び絶対姿勢の信頼度は、例えば、予め実験的に決定された理論誤差（すなわち、位置及び姿勢の真値からの統計的な誤差）のモデルを参照し、撮影装置１１によって撮影された画像に基づいて計算された推定誤差によって表される。信頼度は推定誤差が小さいほど大きくなる。

【0093】

事前実験として、さまざまな条件で撮影装置１１によってマーカー４を撮影し、車両１の実際の位置及び姿勢と、マーカー４に基づいて計算された車両１の絶対位置及び絶対姿勢との誤差を計算する。このとき、マーカー４に対してさまざまな距離及び角度に位置した予め決められた複数の観測点から、撮影装置１１によりマーカー４を撮影する。観測点は、例えば、予め決められた間隔を有する格子点であってもよい。撮影装置１１の向きを変化させながらマーカー４を撮影してもよい。

【0094】

図１５の例では、マーカー４に対して相対的に近接した撮影装置１１－１と、マーカー４に対して相対的に遠隔した撮影装置１１－２とを示す。撮影装置１１－１を備える車両１は、絶対位置の誤差Ｅ２ａ－１と、絶対姿勢の誤差Ｅ２ｂ－１とを有する。また、撮影装置１１－２を備える車両１は、絶対位置の誤差Ｅ２ａ－１より大きい絶対位置の誤差Ｅ２ａ－２と、絶対姿勢の誤差Ｅ２ｂ－１より大きい絶対姿勢の誤差Ｅ２ｂ－２とを有する。従って、図１５によれば、絶対位置及び絶対姿勢の誤差は、撮影装置１１からマーカー４までの距離に依存すると考えられる。

【0095】

図１５の例では、絶対姿勢の誤差Ｅ２ｂ－１，Ｅ２ｂ－２をマーカー座標系に関連付けて示すが、図１２のステップＳ２６において説明したように、マーカー座標系における姿勢は世界座標系における姿勢に換算される。

【0096】

図１６は、絶対位置の誤差が、図１の撮影装置１１によって撮影された画像におけるマーカー４の見かけのサイズに依存することを説明するグラフである。図１７は、絶対姿勢の誤差が、図１の撮影装置１１によって撮影された画像におけるマーカー４の見かけのサイズに依存することを説明するグラフである。図１６及び図１７にプロットした各点は、前述の事前実験の結果を示す。ここで、撮影装置１１によって撮影された画像におけるマーカー４の見かけのサイズは、撮影装置１１からマーカー４までの距離に応じて変化する。図１６及び図１７によれば、マーカー４の縦方向の長さと絶対位置及び絶対姿勢の誤差との間には相関があると考えられる。従って、図１６及び図１７のグラフに曲線をあてはめ、あてはめた曲線に基づいて、絶対位置の理論誤差Ｅ２ａ及び絶対姿勢の理論誤差Ｅ２ｂを例えば以下のように表す。

【0097】

Ｅ２ａ＝４０／縦方向の長さ（画素数）
Ｅ２ｂ＝４００／縦方向の長さ（画素数）

【0098】

このように予め実験的に決定された理論誤差のモデルが与えられ、車両１が任意の経路に沿って移動するとき、絶対位置の理論誤差Ｅ２ａ及び絶対姿勢の理論誤差Ｅ２ｂを、絶対位置及び絶対姿勢の推定誤差として計算することができる。

【0099】

撮影装置１１によって撮影された画像におけるマーカー４の見かけのサイズが小さくなるほど、絶対位置及び絶対姿勢の推定誤差は増大する（すなわち、絶対位置及び絶対姿勢の信頼度が低下する）。また、画像におけるマーカー４の見かけのサイズが大きくなるほど、絶対位置及び絶対姿勢の推定誤差は低下する（すなわち、絶対位置及び絶対姿勢の信頼度が増大する）。従って、絶対位置及び絶対姿勢の推定誤差を用いて、絶対位置及び絶対姿勢の信頼度を表すことができる。

【0100】

第１の実施形態によれば、相対位置及び相対姿勢の信頼度と、絶対位置及び絶対姿勢の信頼度とを計算し、信頼度が高いほうを車両１の位置及び姿勢として決定することにより、車両１の位置及び姿勢を高精度に測定することができる。第１の実施形態によれば、例えば、相対位置及び相対姿勢の信頼度としてそれらの推定誤差を計算し、絶対位置及び絶対姿勢の信頼度としてそれらの推定誤差を計算し、推定誤差が小さいほうを、車両１の位置及び姿勢として決定することができる。

【0101】

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

【0102】

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

【0103】

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

【0104】

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

【0105】

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

【0106】

［第１の実施形態の第１の変形例］
図１８は、第１の実施形態の第１の変形例に係る測位処理における相対位置及び相対姿勢の信頼度の計算を説明する図である。信頼度計算器３６は、図１１を参照して説明したように、車両１の移動距離及び回転角度に基づいて相対位置及び相対姿勢の信頼度を計算することに代えて、相対位置及び相対姿勢の信頼度を以下のように計算してもよい。

【0107】

信頼度計算器３６は、絶対位置及び絶対姿勢に対する相対位置及び相対姿勢の差が小さくなるほど信頼度が増大するように、また、絶対位置及び絶対姿勢に対する相対位置及び相対姿勢の差が大きくなるほど信頼度が低下するように、相対位置及び相対姿勢の信頼度を計算する。この場合、相対位置及び相対姿勢の信頼度は、絶対位置及び絶対姿勢に対する相対位置及び相対姿勢の差（「相対位置及び相対姿勢の推定誤差」ともいう）によって表される。

【0108】

図１８において、車両１は、マーカー４に基づいて計算された絶対位置Ｐ１０及び絶対姿勢Ａ１０を有する。また、車両１は、Visual-SLAMを用いて計算された、相対位置Ｐ１１及び相対姿勢Ａ１１（図１８では「車両１’」として示す）、又は、相対位置Ｐ１２及び相対姿勢Ａ１２（図１８では「車両１”」として示す）を有する。車両１は、絶対位置の推定誤差Ｅ２ａ及び絶対姿勢の推定誤差Ｅ２ｂを有する。

【0109】

信頼度計算器３６は、車両１の相対位置及び絶対位置の間の距離を、車両１の相対位置の推定誤差として計算する。位置及び姿勢決定器３７は、車両１の相対位置及び絶対位置の間の距離が絶対位置の推定誤差Ｅ２ａより大きいとき、絶対位置を車両１の位置として決定する。また、位置及び姿勢決定器３７は、車両１の相対位置及び絶対位置の間の距離が絶対位置の推定誤差Ｅ２ａ以下であるとき、相対位置を車両１の位置として決定する。従って、図１８の例では、車両１が相対位置Ｐ１１を有する場合、車両１の相対位置Ｐ１１は推定誤差ｄ１１＜Ｅ２ａを有し、位置及び姿勢決定器３７は、相対位置Ｐ１１を車両１の位置として決定する。一方、車両１が相対位置Ｐ１２を有する場合、車両１の相対位置Ｐ１２は推定誤差ｄ１２＞Ｅ２ａを有し、位置及び姿勢決定器３７は、絶対位置Ｐ１０を車両１の位置として決定する。

【0110】

信頼度計算器３６は、車両１の相対姿勢及び絶対姿勢の間の角度差を、車両１の相対姿勢の推定誤差として計算する。位置及び姿勢決定器３７は、車両１の相対姿勢及び絶対姿勢の間の角度差が絶対姿勢の推定誤差Ｅ２ｂより大きいとき、絶対姿勢を車両１の姿勢として決定する。また、位置及び姿勢決定器３７は、車両１の相対姿勢及び絶対姿勢の間の角度差が絶対姿勢の推定誤差Ｅ２ｂ以下であるとき、相対姿勢を車両１の姿勢として決定する。従って、図１８の例では、車両１が相対姿勢Ａ１１を有する場合、車両１の相対姿勢Ａ１１は推定誤差θ１＜Ｅ２ｂを有し、位置及び姿勢決定器３７は、相対姿勢Ａ１１を車両１の姿勢として決定する。一方、車両１が相対姿勢Ａ１２を有する場合、車両１の相対姿勢Ａ１２は推定誤差θ２＞Ｅ２ｂを有し、位置及び姿勢決定器３７は、絶対姿勢Ａ１０を車両１の姿勢として決定する。

【0111】

絶対位置及び絶対姿勢に対する相対位置及び相対姿勢の差が大きくなるほど、相対位置及び相対姿勢の推定誤差は増大する（すなわち、相対位置及び相対姿勢の信頼度が低下する）。また、絶対位置及び絶対姿勢に対する相対位置及び相対姿勢の差が小さくなるほど、相対位置及び相対姿勢の推定誤差は低下する（すなわち、相対位置及び相対姿勢の信頼度が増大する）。従って、相対位置及び相対姿勢の推定誤差を用いて、相対位置及び相対姿勢の信頼度を表すことができる。

【0112】

図１８の例では、絶対位置及び絶対姿勢の推定誤差は、図１５～図１７を参照して説明した場合と同様に計算される。

【0113】

第１の実施形態の第１の変形例によれば、相対位置及び相対姿勢の推定誤差と、絶対位置及び絶対姿勢の推定誤差とを計算し、推定誤差が小さいほう（すなわち、信頼度が高いほう）を、車両１の位置及び姿勢として決定することにより、車両１の位置及び姿勢を高精度に測定することができる。

【0114】

［第１の実施形態の第２の変形例］
図１９は、第１の実施形態の第２の変形例に係る測位処理における相対位置及び相対姿勢の信頼度の計算を説明する図である。信頼度計算器３６は、図１１又は図１８を参照して説明したように相対位置及び相対姿勢の推定誤差を計算する際に、トラッキングの信頼度を考慮してもよい。

【0115】

相対位置計算器３２は、時間的に隣接した複数の画像においてトラッキングされた特徴点に基づいて相対位置及び相対姿勢を計算する。信頼度計算器３６は、時間的に隣接した複数の画像においてトラッキングに成功した特徴点の個数に基づいてトラッキングの信頼度を計算し、トラッキングの信頼度が高くなるほど相対位置及び相対姿勢の信頼度が増大するように相対位置及び相対姿勢の信頼度を計算する。この場合、相対位置及び相対姿勢の信頼度は、例えば、図１１又は図１８を参照して説明した場合と同様に計算された相対位置及び相対姿勢の推定誤差に対して、トラッキングの信頼度を示す係数を乗算した値によって表される。

【0116】

図１９（ａ）を参照すると、画像４０（ｎ）において特徴点Ｆ１～Ｆ４が検出されている。図１９（ｂ）を参照すると、画像４０（ｎ’）において、図１９（ａ）の特徴点Ｆ２に対応する特徴点Ｆ２’と、図１９（ａ）の特徴点Ｆ４に対応する特徴点Ｆ４’とが検出されているが、図１９（ａ）の特徴点Ｆ１，Ｆ３に対応する特徴点の検出に失敗している。

【0117】

トラッキングを考慮した相対位置の推定誤差Ｅ１ａ’及び相対姿勢の推定誤差Ｅ１ｂ’は、次式によって計算される。

【0118】

Ｅ１ａ’＝λ×（１／Ｔｆ）×Ｅ１ａ
Ｅ１ｂ’＝λ×（１／Ｔｆ）×Ｅ１ｂ

【0119】

λは予め決められた定数である。Ｔｆはトラッキングの信頼度を示し、次式によって計算される。

【0120】

Ｔｆ＝トラッキングに成功した特徴点の総数／車両１の移動距離（ｍ）

【0121】

例えば、車両１が移動距離１０ｍにわたって移動し、時間的に隣接した２つの画像において４００００個の特徴点のトラッキングに成功し、λ＝３０００の場合、相対位置及び相対姿勢の推定誤差は以下のように計算される。

【0122】

Ｔｆ＝４００００／１０＝４０００
Ｅ１ａ’＝３０００×（１／４０００）×Ｅ１ａ
Ｅ１ｂ’＝３０００×（１／４０００）×Ｅ１ｂ

【0123】

相対位置の推定誤差Ｅ１ａ及び相対姿勢の推定誤差Ｅ１ｂは、図１１又は図１８を参照して説明した場合と同様に計算される。

【0124】

トラッキングの信頼度が低くなるほど、相対位置及び相対姿勢の推定誤差は増大する（すなわち、相対位置及び相対姿勢の信頼度が低下する）。また、トラッキングの信頼度が高くなるほど、相対位置及び相対姿勢の推定誤差は低下する（すなわち、相対位置及び相対姿勢の信頼度が増大する）。従って、トラッキングの信頼度を考慮した相対位置及び相対姿勢の推定誤差を用いて、相対位置及び相対姿勢の信頼度を表すことができる。

【0125】

絶対位置及び絶対姿勢の信頼度は、図１５～図１７を参照して説明した場合と同様に計算される。

【0126】

第１の実施形態の第２の変形例によれば、トラッキングの信頼度を考慮することにより、車両１の位置及び姿勢をより高精度に測定することができる。

【0127】

［第１の実施形態の第３の変形例］
信頼度計算器３６は、相対位置及び相対姿勢の信頼度として、図１１、図１８、又は図１９を参照して説明した場合と同様に相対位置及び相対姿勢の推定誤差を計算することに代えて、相対位置及び相対姿勢の変化量の分散を計算してもよい。

【0128】

信頼度計算器３６は、相対位置又は相対姿勢の変化量の分散が小さくなるほど信頼度が増大するように、また、相対位置又は相対姿勢の変化量の分散が大きくなるほど信頼度が低下するように、相対位置及び相対姿勢の信頼度を計算する。この場合、相対位置及び相対姿勢の信頼度は、相対位置又は相対姿勢の変化量の分散によって表される。

【0129】

図２０は、第１の実施形態の第３の変形例に係る測位処理における相対位置及び相対姿勢の信頼度の計算を説明する図であって、車両１の第１の軌跡を示す図である。図２１は、図２０の車両１の相対位置又は相対姿勢の変化量を概略的に示すグラフである。図２０に示すように、時刻ｎ－２～ｎ＋２において、車両１は相対位置Ｐ（ｎ－２）～Ｐ（ｎ＋２）及び相対姿勢Ａ（ｎ－２）～Ａ（ｎ＋２）を有する。この場合、相対位置及び相対姿勢の変化量は小さく、その分散も小さい。

【0130】

図２２は、第１の実施形態の第３の変形例に係る測位処理における相対位置及び相対姿勢の信頼度の計算を説明する図であって、車両１の第２の軌跡を示す図である。図２３は、図２２の車両１の相対位置又は相対姿勢の変化量を概略的に示すグラフである。図２２に示すように、時刻ｎ－２～ｎ＋２において、車両１は相対位置Ｐ（ｎ－２）～Ｐ（ｎ＋２）及び相対姿勢Ａ（ｎ－２）～Ａ（ｎ＋２）を有する。この場合、相対位置及び相対姿勢の変化量は図２０の場合よりもフレームごとに大きく変動し、その分散も図２０の場合より大きくなる。

【0131】

相対位置及び相対姿勢の変化量がフレームごとに大きく変動するとき、相対位置及び相対姿勢を高精度に計算することは困難であると考えられる。相対位置又は相対姿勢の変化量の分散が大きくなるほど、相対位置及び相対姿勢の信頼度が低下する。また、相対位置又は相対姿勢の変化量の分散が小さくなるほど、相対位置及び相対姿勢の信頼度が増大する。従って、相対位置及び相対姿勢の変化量の分散を用いて、相対位置及び相対姿勢の信頼度を表すことができる。

【0132】

絶対位置及び絶対姿勢の信頼度は、図１５～図１７を参照して説明した場合と同様に計算される。

【0133】

第１の実施形態の第３の変形例によれば、相対位置及び相対姿勢の信頼度としてそれらの分散の変化量を計算し、絶対位置及び絶対姿勢の信頼度としてそれらの推定誤差を計算し、信頼度が高いほうを車両１の位置及び姿勢として決定することができる。

【0134】

［第１の実施形態の第４の変形例］
図２４は、第１の実施形態の第４の変形例に係る測位処理における絶対位置及び絶対姿勢の信頼度の計算を説明する図である。信頼度計算器３６は、図１５～図１７を参照して説明したように絶対位置及び絶対姿勢の推定誤差を計算する際に、撮影装置１１によって撮影された画像におけるマーカー４の位置を考慮してもよい。

【0135】

図２４を参照すると、撮影装置１１によって撮影された画像４０において、マーカー４は、画像４０の中心Ｐ２１から距離ｄ２１の位置にある。信頼度計算器３６は、撮影装置１１によって撮影された画像４０において、画像４０の中心Ｐ２１からマーカー４までの距離ｄ２１が小さくなるほど信頼度が増大するように、また、距離ｄ２１が大きくなるほど信頼度が低下するように、絶対位置及び絶対姿勢の信頼度を計算する。この場合、絶対位置及び絶対姿勢の信頼度は、例えば、図１５～図１７を参照して説明した場合と同様に計算された絶対位置及び絶対姿勢の推定誤差に対して、画像４０の中心Ｐ２１からマーカー４までの距離ｄ２１を含む係数を乗算した値によって表される。

【0136】

画像におけるマーカー４の位置を考慮した絶対位置の推定誤差Ｅ２ａ’及び絶対姿勢の推定誤差Ｅ２ｂ’は、次式によって表される。

【0137】

Ｅ２ａ’＝λ×ｄ２１×Ｅ２ａ
Ｅ２ｂ’＝λ×ｄ２１×Ｅ２ｂ

【0138】

λは予め決められた定数である。絶対位置の推定誤差Ｅ２ａ及び絶対姿勢の推定誤差Ｅ２ｂは、図１５～図１７を参照して説明した場合と同様に計算される。

【0139】

相対位置及び相対姿勢の信頼度は、図１１、図１８～図２３を参照して説明した場合と同様に計算される。

【0140】

撮影装置１１によって撮影された画像におけるマーカー４の位置によっては、撮影装置１１の光軸に対して異なる角度の面を有するマーカー４が区別しにくくなることがある。例えば、撮影装置１１によって撮影された画像の端部では、撮影装置１１の光軸に対して垂直な面を有するマーカー４と、平行な面を有するマーカー４とが区別しにくくなる。

【0141】

図２５は、図１の撮影装置１１によって撮影されるマーカー４ａ－１～４ａ－１２の例示的な配置を示す図である。各マーカー４ａ－１～４ａ－１２は、０．６ｍ×０．６ｍの正方形形状を有する。また、各マーカー４ａ－１～４ａ－１２は、それらの面が撮影装置１１の光軸に対して垂直に、すなわち、Ｘｗ－Ｚｗ平面に対して平行に配置される。図２６は、図２５のマーカー４ａ－１～４ａ－１２を撮影装置１１によって撮影した画像４０の例を示す図である。

【0142】

図２７は、図１の撮影装置１１によって撮影されるマーカー４ｂ－１～４ｂ－１２の例示的な配置を示す図である。各マーカー４ｂ－１～４ｂ－１２は、０．６ｍ×０．６ｍの正方形形状を有する。また、各マーカー４ｂ－１～４ｂ－１２は、それらの面が撮影装置１１の光軸に対して平行に、すなわち、Ｙｗ－Ｚｗ平面に対して平行になるように配置される。図２８は、図２７のマーカー４ｂ－１～４ｂ－１２を撮影装置１１によって撮影した画像４０の例を示す図である。図２８を図２６と比較すると、図２８のマーカー４ｂ－６は、面の向きが９０度異なるにもかかわらず、図２６のマーカー４ａ－１～４ａ－１２のそれぞれとほとんど同じサイズ及び形状に見えることがわかる。

【0143】

図２９は、図１の撮影装置１１の光軸に対するマーカー４の面の角度を誤検出したことに起因する、マーカー座標系における撮影装置１１の位置の変動を説明する図である。前述のように、図２６のマーカー４ａ－１～４ａ－１２及び図２８のマーカー４ｂ－６は、見かけ上、互いに区別することができない。従って、撮影装置１１は、図２９のマーカー座標系において、「１１ａ」の位置及び姿勢を有するべきところ、「１１ｂ」の位置及び姿勢を有すると誤って判断されることがあり、また、その逆の場合もある。撮影装置１１の光軸がマーカー４の面の実質的に中心を通りかつマーカー４の面に実質的に直交する場合、当該マーカー４を原点とするマーカー座標系における撮影装置１１の位置を決定することは困難であり、当該マーカー４は車両１の絶対位置及び絶対姿勢の計算に不適である。従って、撮影装置１１の光軸に対して垂直な面を有するマーカー４（図２５のマーカー４－１～４ａ－１２）と、撮影装置１１の光軸に対して垂直な面を有するように見えるマーカー４（図２７のマーカー４ｂ－６）との信頼性が低いと考えられる。信頼度計算器３６は、このようなマーカー４の絶対位置及び絶対姿勢の推定誤差を増大させる。

【0144】

画像４０の中心Ｐ２１からマーカー４までの距離ｄ２１が大きくなるほど（すなわち、画像４０においてマーカー４が端部に近づくほど）、絶対位置及び絶対姿勢の推定誤差は増大する（すなわち、絶対位置及び絶対姿勢の信頼度が低下する）。また、距離ｄ２１が小さくなるほど（すなわち、画像４０においてマーカー４が中心Ｐ２１に近づくほど）、絶対位置及び絶対姿勢の推定誤差は低下する（すなわち、絶対位置及び絶対姿勢の信頼度が増大する）。従って、画像におけるマーカー４の位置を考慮した絶対位置及び絶対姿勢の推定誤差を用いて、絶対位置及び絶対姿勢の信頼度を表すことができる。

【0145】

図２４の変形例によれば、撮影装置１１によって撮影された画像におけるマーカー４の位置を考慮することにより、車両１の位置及び姿勢をより高精度に測定することができる。

【0146】

［第１の実施形態の第５の変形例］
図３０は、第１の実施形態の第５の変形例に係る測位処理における絶対位置及び絶対姿勢の信頼度の計算を説明する図である。信頼度計算器３６は、撮影装置１１によって撮影された画像におけるマーカー４の見かけのサイズを考慮してもよい。

【0147】

信頼度計算器３６は、撮影装置１１によって撮影された画像におけるマーカー４の見かけのサイズが大きくなるほど信頼度が増大するように、また、画像におけるマーカー４の見かけのサイズが小さくなるほど信頼度が低下するように、絶対位置及び絶対姿勢の信頼度を計算してもよい。この場合、絶対位置及び絶対姿勢の信頼度は、例えば、図１５～図１７、図２４～図２９を参照して説明した場合と同様に計算された絶対位置及び絶対姿勢の推定誤差に対して、画像におけるマーカー４の見かけのサイズを示す係数を乗算した値によって表される。

【0148】

図３０を参照すると、撮影装置１１によって撮影された画像４０において、マーカー４ａは見かけのサイズｄ３１を有し、マーカー４ｂは見かけのサイズｄ３２を有する。画像におけるマーカー４ａ，４ｂの見かけのサイズは、例えば、縦方向の長さ（画素数）によって表される。画像におけるマーカー４の見かけのサイズは、撮影装置１１からマーカー４までの距離に対応する。

【0149】

絶対位置の推定誤差Ｅ２ａ及び絶対姿勢の推定誤差Ｅ２ｂは、図１５～図１７、図２４～図２９を参照して説明した場合と同様に計算される。

【0150】

相対位置及び相対姿勢の信頼度は、図１１、図１８～図２３を参照して説明した場合と同様に計算される。

【0151】

画像におけるマーカー４の見かけのサイズが小さくなるほど（すなわち、撮影装置１１からマーカー４までの距離が大きくなるほど）、絶対位置及び絶対姿勢の推定誤差は増大する（すなわち、絶対位置及び絶対姿勢の信頼度が低下する）。また、画像におけるマーカー４の見かけのサイズが大きくなるほど（すなわち、撮影装置１１からマーカー４までの距離が小さくなるほど）、絶対位置及び絶対姿勢の推定誤差は低下する（すなわち、絶対位置及び絶対姿勢の信頼度が増大する）。従って、画像におけるマーカー４の見かけのサイズを考慮した絶対位置及び絶対姿勢の推定誤差を用いて、絶対位置及び絶対姿勢の信頼度を表すことができる。

【0152】

追加又は代替として、位置及び姿勢決定器３７は、撮影装置１１によって撮影された画像におけるマーカー４の見かけのサイズが予め決められた閾値よりも小さいとき、常に、相対位置及び相対姿勢を車両１の位置及び姿勢として決定してもよい。言いかえると、画像におけるマーカー４の見かけのサイズが閾値よりも小さいとき、絶対位置及び絶対姿勢の信頼度が最低値になる。画像におけるマーカー４の見かけのサイズの閾値は、例えば、縦方向において７画素に設定される。

【0153】

図３０の変形例によれば、撮影装置１１によって撮影された画像におけるマーカー４の見かけのサイズを考慮することにより、車両１の位置及び姿勢をより高精度に測定することができる。

【0154】

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

【0155】

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

【0156】

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

【0157】

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

【0158】

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

【0159】

車両１は、フォークリフトに代えて、例えばトラック又は牽引車など、有人の乗物であってもよい。また、車両１は、例えば無人搬送車（ＡＧＶ）又はパレット搬送ロボットなど、無人の荷物運搬装置であってもよい。この場合、車両１は、サーバ装置２の制御下で駆動機構１５を制御して移動する。また、車両１は、手押し台車など、駆動機構を持たない人力の車両であってもよい。

【0160】

測位装置１２は、車両１に代えて、サーバ装置２に設けられてもよい。この場合、撮影装置１１によって撮影された画像は、通信装置１３（又は着脱可能な記憶媒体）によって車両１からサーバ装置２に送られる。サーバ装置２の測位装置１２は、車両１から取得された画像に基づいて、図２の測位装置１２と同様に、車両１の位置及び姿勢を計算する。

【0161】

［第１の実施形態の効果等］
第１の実施形態によれば、測位装置１２は、相対位置計算器３２と、記憶装置３５と、絶対位置計算器３４と、信頼度計算器３６と、位置及び姿勢決定器３７とを備える。相対位置計算器３２は、車両１に搭載された撮影装置１１によって撮影された複数の画像に基づいて、所定の基準位置及び基準姿勢に対する車両１の相対位置及び相対姿勢を示す車両１の相対位置及び相対姿勢を計算する。記憶装置３５は、予め決められた位置に配置されて視覚的に識別可能な複数のマーカー４の識別子、位置、及び姿勢の情報と、車両１のための通路を含むマップの情報とを格納する。絶対位置計算器３４は、撮影装置１１によって撮影された画像から複数のマーカー４のうちの１つを抽出し、抽出された１つのマーカー４の位置及び姿勢に基づいて、マップにおける車両１の位置及び姿勢を示す車両１の絶対位置及び絶対姿勢を計算する。信頼度計算器３６は、相対位置計算器３２によって計算された相対位置及び相対姿勢の信頼度と、絶対位置計算器３４によって計算された絶対位置及び絶対姿勢の信頼度とを計算する。位置及び姿勢決定器３７は、相対位置及び相対姿勢の信頼度が絶対位置及び絶対姿勢の信頼度以上であるとき、相対位置及び相対姿勢を車両１の位置及び姿勢として決定し、相対位置及び相対姿勢の信頼度が絶対位置及び絶対姿勢の信頼度より小さいとき、絶対位置及び絶対姿勢を車両１の位置及び姿勢として決定する。

【0162】

これにより、相対位置及び相対姿勢の信頼度と、絶対位置及び絶対姿勢の信頼度とを計算し、信頼度が高いほうを車両１の位置及び姿勢として決定することにより、車両１の位置及び姿勢を高精度に測定することができる。

【0163】

第１の実施形態によれば、信頼度計算器３６は、車両１の基準位置及び基準姿勢からの移動距離及び回転角度が小さくなるほど信頼度が増大するように相対位置及び相対姿勢の信頼度を計算してもよい。位置及び姿勢決定器３７が絶対位置及び絶対姿勢を車両１の位置及び姿勢として決定したとき、信頼度計算器３６は基準位置及び基準姿勢をリセットする。

【0164】

これにより、車両１の移動距離及び回転角度に基づいて、相対位置及び相対姿勢の誤差を計算することができる。

【0165】

第１の実施形態によれば、信頼度計算器３６は、絶対位置及び絶対姿勢に対する相対位置及び相対姿勢の差が小さくなるほど信頼度が増大するように相対位置及び相対姿勢の信頼度を計算してもよい。

【0166】

これにより、絶対位置及び絶対姿勢に対する相対位置及び相対姿勢の差に基づいて、相対位置及び相対姿勢の誤差を計算することができる。

【0167】

第１の実施形態によれば、相対位置計算器３２は、時間的に隣接した複数の画像においてトラッキングされた特徴点に基づいて相対位置及び相対姿勢を計算してもよい。信頼度計算器３６は、時間的に隣接した複数の画像においてトラッキングに成功した特徴点の個数に基づいてトラッキングの信頼度を計算し、トラッキングの信頼度が高くなるほど相対位置及び相対姿勢の信頼度が増大するように相対位置及び相対姿勢の信頼度を計算してもよい。

【0168】

これにより、トラッキングの信頼度に基づいて、相対位置及び相対姿勢の誤差を計算することができる。

【0169】

第１の実施形態によれば、信頼度計算器３６は、相対位置又は相対姿勢の変化量の分散が小さくなるほど信頼度が増大するように相対位置及び相対姿勢の信頼度を計算してもよい。

【0170】

これにより、相対位置又は相対姿勢の変化量の分散に基づいて、相対位置及び相対姿勢の誤差を計算することができる。

【0171】

第１の実施形態によれば、信頼度計算器３６は、撮影装置１１によって撮影された画像において、画像の中心からマーカー４までの距離が小さくなるほど信頼度が増大するように絶対位置及び絶対姿勢の信頼度を計算してもよい。

【0172】

これにより、撮影装置１１によって撮影された画像におけるマーカー４の位置を考慮して、絶対位置及び絶対姿勢の誤差を計算することができる。

【0173】

第１の実施形態によれば、信頼度計算器３６は、撮影装置１１によって撮影された画像におけるマーカー４の見かけのサイズが大きくなるほど信頼度が増大するように絶対位置及び絶対姿勢の信頼度を計算してもよい。

【0174】

これにより、撮影装置１１によって撮影された画像におけるマーカー４の見かけのサイズに基づいて、絶対位置及び絶対姿勢の誤差を計算することができる。

【0175】

第１の実施形態によれば、位置及び姿勢決定器３７は、撮影装置１１によって撮影された画像におけるマーカー４の見かけのサイズが予め決められた閾値よりも小さいとき、相対位置及び相対姿勢を車両１の位置及び姿勢として決定してもよい。

【0176】

これにより、撮影装置１１によって撮影された画像におけるマーカー４の見かけのサイズを考慮して、絶対位置及び絶対姿勢の誤差を計算することができる。

【0177】

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

【0178】

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

【0179】

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

【0180】

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

【0181】

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

【0182】

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

【0183】

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

【0184】

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

【0185】

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

【0186】

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

【0187】

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

【0188】

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

【0189】

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

【0190】

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

【0191】

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

【産業上の利用可能性】

【0192】

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

【符号の説明】

【0193】

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

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】

【図18】

【図19】

【図20】

【図21】

【図22】

【図23】

【図24】

【図25】

【図26】

【図27】

【図28】

【図29】

【図30】

【図31】