JP2015225014A - 位置推定装置、位置推定方法、およびプログラム - Google Patents

Abstract

【課題】精度よく実空間内の位置を推定する。
【解決手段】マーカ１４−１〜１４−Ｎを実空間内に固定する。撮影装置１３は基準点に対する相対位置が定められた撮影位置から周辺画像を撮影する。位置推定装置１１のマーカ位置取得部１１３は、撮影された周辺画像に由来する複数個のマーカの画像情報に基づいて複数個のマーカの位置情報を得る。位置推定装置１１の位置推定部１１４は、これらの複数個のマーカの位置情報に基づいて基準点の位置推定値を得る。
【選択図】図１

Description

本発明は、位置推定を行う技術に関する。

屋外ではＧＰＳ（Global Positioning System）のような人工衛星を利用したシステムによって１ｍオーダーの精度で位置情報を推定できる。しかしながら、ＧＰＳを用いた位置推定には３台以上の人工衛星からの直接波が必要であるため、これを屋内で使用することはできない。

屋内において客体の位置推定を行う方式として、移動環境に多数のカメラを配置して当該客体に固定されたマーカを捉えることでその位置検出を行う方式（例えば、モーションキャプチャ等）がある。しかしながら、このような方式では、位置推定を行う領域すべてをカバーするように高価なカメラを配置しなければならない。また、マーカがカメラ視野から隠蔽されないように、この領域に入る人の数がかなり制限される。

また、カメラで撮影したＡＲ（augmented reality）マーカ等のマーカを画像処理し、それに基づいて位置推定を行う手法もある（例えば、非特許文献１参照）。マーカは円形形状やＱＲコード（登録商標）を含む図形等であり（例えば、特許文献１参照）、撮影画像中のマーカ位置に仮想物体を重畳表示するために用いられる（例えば、非特許文献１参照）。

特開２００８−２７１５７号公報

H. Kato and M. Billinghurst, "Marker tracking and hmd calibration for a video-based augmented reality conferencing system," Proc. IWAR, pp. 85-94, 1999.

しかしながら、このような手法は撮影画像からなる仮想空間内のマーカの位置を推定するものであり、実空間内の位置を推定するものではない。さらに、このような手法では、原理的に平面に位置するマーカ上の４点（独立な方程式が４個）の情報を取得できれば利用できるが、実際の用途ではカメラの解像度やレンズの歪みなどの誤差によって十分な精度は保証できない。

本発明はこのような点に鑑みてなされたものであり、精度よく実空間内の位置を推定する技術を提供することを目的とする。

本発明では上記課題を解決するために、基準点に対する相対位置が定められた撮影位置から撮影された周辺画像に由来する複数個のマーカの画像情報に基づいて複数個のマーカの位置情報を得、複数個のマーカの位置情報に基づいて基準点の位置推定値を得る。

本発明では、複数個のマーカの位置情報に基づいて基準点の位置推定値を得るため、精度よく実空間内の位置を推定できる。

図１は実施形態の位置推定システムの構成を例示したブロック図である。 図２Ａから図２Ｃはマーカを例示した図である。 図３Ａから図３Ｆは画像処理の流れを例示するための図である。 図４Ａから図４Ｃは加速度発生装置を例示した図である。 図５Ａおよび図５Ｂは加速度発生装置を構成する振動子を例示した図である。 図６はカメラと加速度発生装置とが一体化された例を示した斜視図である。 図７は実施形態の処理を説明するためのフロー図である。 図８は実施形態の処理を説明するためのフロー図である。 図９は実施形態のマーカを例示した図である。

以下、本発明の実施形態を説明する。
［概要］
実施形態の位置推定装置では、基準点に対する相対位置が定められた撮影位置から撮影された周辺画像に由来する複数個のマーカ（基準マーカ）の画像情報に基づいて複数個のマーカの位置情報を得、複数個のマーカの位置情報に基づいて基準点の位置推定値を得る。位置推定値は実空間での座標（世界座標系における座標）を表す。基準点は撮影位置に一致していてもよいし、一致していなくてもよい。撮影位置は、例えば、周辺画像を撮影する撮影装置（カメラ等）のレンズの中心位置や撮像素子の中心位置などである。また、マーカは実空間に配置された模様を有する有体物である。マーカの例はＡＲマーカである。マーカは印刷物や塗装物であってもよいし、色や反射特定が異なる複数の部材からなる固形物（タイル等）であってもよいし、画面に表示された表示（液晶表示等）であってもよい。また、人間の目では区別できないが特定の撮影装置（赤外線カメラや紫外線カメラ等）で撮影された画像では区別可能なマーカであってもよい。すなわち、反射した可視光からは模様を識別できないが、反射した赤外線や紫外線からは模様を識別できるマーカであってもよい。このような場合の周辺画像は、人間の目では区別できないマーカの相違を区別して撮影できる撮影装置で得られた画像となる。複数個のマーカは実空間内の互いに異なる位置に配置される。マーカの実空間での位置（世界座標系における座標）は固定である。各実施形態では、複数個のマーカの位置情報を用いることで、高い精度で実空間の位置情報を推定する。

マーカの画像情報は、例えば、マーカの世界座標系における座標を特定するための情報を表す。例えば、位置推定装置がマーカの世界座標系における座標とマーカの識別子と対応付けられたデータベースを格納している場合、マーカの識別子を「マーカの世界座標系における座標を特定するための情報」として用いることができる。或いは、マーカの世界座標系における座標そのものを表す情報を「マーカの世界座標系における座標を特定するための情報」としてもよい。この場合のマーカの位置情報は、例えば、このマーカの世界座標系における座標を特定するための情報から特定されるマーカの世界座標系における座標と、マーカの画像情報から特定されるマーカの画像座標系における座標とを含む。なお、世界座標系は実空間の三次元座標系（実空間の原点を基準とした三次元座標系）であり、画像座標系は周辺画像の二次元座標系（画像内の原点を基準とした二次元座標系）である。

実空間内の複数の平面のそれぞれにマーカが位置し、撮影された周辺画像が複数の平面のそれぞれに位置するマーカの画像を含んでいてもよい。この場合には、同一の平面に位置する複数個のマーカの画像に由来する複数個のマーカの画像情報に基づいて基準点の位置の推定値（「予備推定値」と呼ぶことにする）を得、平面のそれぞれに対して得られた予備推定値を用いて位置推定値を得る。これにより、より高い精度で実空間の位置情報を推定できる。なお、実空間内の平面には１個のマーカのみが配置されていてもよいし、複数個のマーカが配置されていてもよい。より高い精度で位置推定を行うためには、各平面に複数個のマーカが配置されていることが望ましい。

また、実空間内の目的位置の情報と上述のように得られた位置推定値とを用い、基準点または基準点に対する所定の相対位置を基点とした目的位置の方向（方位）を推定してもよい。さらに、この目的位置の方向成分を持つ向きに、振動による方向提示を行うための制御情報を生成して出力してもよい。この制御情報を加速度発生装置に入力することで、加速度発生装置は目的位置の方向成分を持つ向きに振動による方向提示を行う。これにより、加速度発生装置を把持するユーザを目的位置に誘導することができる。なお、人間は多くの情報の収集を視覚器官に頼っている。ユーザが歩きながら位置推定装置を利用する場合、目的位置に誘導するための情報を地図等に表示するとユーザの注意が散漫になり、大変危険である。このように振動刺激、つまり触覚を通じて情報を返すことで、ユーザの注意の遷移を小さくし、安全性を向上させることができる。振動による方向提示の例は、その方向への疑似的な力覚の提示である。擬似的な力覚とは、実際には物体が並進運動をしていないにも関わらず、あたかも並進方向へ動きそうな力が働いているような知覚が生成されることをいう。このような疑似的な力覚の提示は、所望の方向の加速度の時間変化とその逆方向の加速度の時間変化とが非対称な偏加速度運動によって実現できる。この詳細は後述する。

位置推定値の精度は、その算出に用いられるマーカの位置情報に依存する。この位置推定値の精度の高さを振動刺激によって提示してもよい。つまり、位置推定値の精度が高い場合には、振動による方向提示をより明確に行ってもよい。言い換えると、位置推定値の精度が低い場合には、振動による方向提示を曖昧に行ってもよい。あるいは、位置推定値の精度が高い場合には、振動による方向提示をより強くしてもよい。言い換えると、位置推定値の精度が低い場合には、振動による方向提示を弱くしてもよい。すなわち、位置推定値の精度を表す推定精度値が「第１値」のときの制御情報が「第１の方向提示」を行うための情報であり、この推定精度値が「第２値」のときの制御情報が「第２の方向提示」を行うための情報とする場合、「第２値」が表す精度が「第１値」が表す精度よりも高く、「第２の方向提示」が「第１の方向提示」よりも明確または強くてもよい。これにより、位置推定の精度を知覚させることができる。さらに、位置推定の精度に応じて振動によって提示する方向の明確性を変化させたり、方向提示の強さを変化させたりもできる。

上述のような振動による方向提示に起因し、撮影された周辺画像に「ぶれ」が生じる可能性がある。そのため、撮影位置の振動を表す情報を用いて周辺画像の振動成分を補正し、それによって得られる補正画像に由来するマーカの画像情報を用い、上述の処理を行ってもよい。これにより、位置推定の精度や方向提示の精度をより向上させることができる。

以下、図面を参照して各実施形態を説明する。
［第１実施形態］
図１に例示するように、本形態の位置推定システム１は、位置推定装置１１と加速度発生装置１２と撮影装置１３と複数個のマーカ１４−１〜１４−Ｎとを有する。ただし、Ｎは２以上の整数である。

＜マーカ１４−１〜１４−Ｎ＞
図２Ａおよび図２Ｂに例示するように、本実施形態のマーカ１４−ｉ（ただし、ｉ＝１，・・・，Ｎ）は、それぞれ、白黒の二色で描かれた頂点を有する多角形の図形である。マーカ１４−ｉには個別の識別子ＩＤ_ｉが振られていている。それぞれのマーカ１４−ｉの特定の領域１４２内に描かれた白や黒の図形がビット列を表現し、そのビット列がそれぞれのマーカ１４−ｉの識別子ＩＤ_ｉ（マーカの世界座標系における座標を特定するための情報）を表している。例えば、特定の領域１４２内に描かれた白および黒のピクセルの比率によって識別子ＩＤ_ｉを表す。さらに、その他の領域１４３内の白や黒の図形が識別子ＩＤ_ｉの誤り検出ビットを表していてもよい。また、鏡やガラスで反射されたマーカの反射像を撮影することによる誤検出を防ぐため、識別子ＩＤ_ｉを表す図形は左右非対称であることが望ましい（例えば、図２Ｂ参照）。また、マーカ１４−ｉの白黒が反転していてもよい。例えば、図２Ｃは図２Ｂのマーカ１４−ｉの白黒を反転させたものである。図２Ｂのマーカ１４−ｉは外枠１４５が黒色であるため、これを黒または黒に近い平面に貼り付けると、マーカ１４−ｉを背景から分離して認識することが困難となる。一方、図２Ｃのマーカ１４−ｉは外枠１４６が白色に反転しているため、これを黒または黒に近い平面に貼り付けた場合でもマーカ１４−ｉを背景から分離して認識することができる。このように、マーカ１４−ｉと外枠と背景色とのコントラスト比が高ければ高いほど、安定してマーカ１４−ｉを背景から分離して認識することができる。なお、以下の説明では、正方形に切り欠き１４１が設けられた外枠のマーカ１４−ｉを例示するが、切り欠きのない正方形の外枠のマーカ１４−ｉであってもよい。

＜位置推定装置１１＞
本実施形態の位置推定装置１１は、記憶部１１１と目的位置生成部１１２とマーカ位置取得部１１３と位置推定部１１４と方位推定部１１５と制御部１１６とを有する。位置推定装置１１は、例えば、ＣＰＵ（central processing unit）等のプロセッサ（ハードウェア・プロセッサ）やＲＡＭ（random-access memory）・ＲＯＭ（read-only memory）等のメモリ等を備える汎用または専用のコンピュータが所定のプログラムを実行することで構成される装置である。このプログラムはコンピュータにインストールされてもよいし、予めＲＯＭ等に記録されていてもよい。また、ＣＰＵのようにプログラムが読み込まれることで機能構成を実現する電子回路（circuitry）ではなく、単独で処理機能を実現する電子回路を用いて一部またはすべての処理部が構成されてもよい。

＜加速度発生装置１２＞
加速度発生装置１２は、振動による方向提示を行う装置である。加速度発生装置１２は、所望の方向の加速度の時間変化とその逆方向の加速度の時間変化とが非対称な偏加速度運動を行う振動子によって所望の方向に擬似的な力覚を提示する（例えば、特許第４５５１４４８号公報参照）。１個の振動子では所望の方向とその逆方向への方向提示を行うことができるが、このような振動子の向きを任意の方向に変化できるようにすることで任意の方向に擬似的な力覚を提示することができる。例えば、図４Ａおよび図４Ｂに例示する加速度発生装置１２は、外部ケース１２８と振動子１２０−１を有し、外部ケース１２８に対する振動子１２０−１の向きを変化させることが可能である。これにより、振動子１２０−１がＣ_１方向に擬似的な力覚を提示していたとしても、図４Ａおよび図４Ｂのように異なる向きに擬似的な力覚を提示することができる。また、このような振動子を複数個設け、それらを仮想的な凸多角形の各辺の位置に配置し、各振動子の駆動を制御することで多方向への擬似的な力覚の提示を可能としてもよい。例えば、図４Ｃに例示する加速度発生装置１２は、外部ケース１２８と振動子１２０−１〜１２０−４を有する。振動子１２０−１〜１２０−４は仮想的な四角形の各辺に配置される。例えば、振動子１２０−２および１２０−４がＤ_１方向に擬似的な力覚を提示するように制御し、振動子１２０−１および１２０−３が擬似的な力覚を提示しないように制御すると、全体としてＤ_１方向に擬似的な力覚を提示できる。一方、振動子１２０−１および１２０−３がＤ_２方向に擬似的な力覚を提示するように制御し、振動子１２０−２および１２０−４が擬似的な力覚を提示しないように制御すると、全体としてＤ_２方向に擬似的な力覚を提示できる。

図５Ａおよび図５Ｂは、振動子１２０−ｉ（ただし、ｉは正整数）の一例を例示した概念図であり、その長手方向の断面を例示している。図５Ａおよび図５Ｂに例示するように、この例の振動子１２０−ｉは、例えば、支持部１２１−ｉ、ばね１２２−ｉ，１２３−ｉ、コイル１２４−ｉ、永久磁石１２５−ｉ、およびケース１２６−ｉを有している。ケース１２６−ｉおよび支持部１２１−ｉは、ともに円筒の両方の開放端を閉じた形状からなる中空の部材である。ただし、支持部１２１−ｉは、ケース１２６−ｉよりも小さく、ケース１２６−ｉの内部に収容可能な大きさである。ケース１２６−ｉおよび支持部１２１−ｉは、例えば、ＡＢＳ樹脂等の合成樹脂から構成される。ばね１２２−ｉ，１２３−ｉは、例えば、金属等から構成されるつるまきばねや板ばね等である。ばね１２２−ｉ，１２３−ｉのばね定数は同一であることが望ましいが、互いに相違していてもよい。永久磁石１２５−ｉは、例えば、円柱形状の永久磁石であり、長手方向の一方の端部１２５ａ−ｉ側がＮ極であり、他方の端部１２５ｂ−ｉ側がＳ極である。コイル１２４−ｉは、例えば、一つながりのエナメル線であり、第１巻き部１２４ａ−ｉと第２巻き部１２４ｂ−ｉとを有する。

永久磁石１２５−ｉは支持部１２１−ｉの内部に収容され、そこで長手方向にスライド可能に支持されている。このような支持機構の詳細は図示しないが、例えば、支持部１２１−ｉの内壁面に長手方向に沿ったまっすぐなレールが設けられ、永久磁石１２５−ｉの側面にこのレールをスライド可能に支持するレール支持部が設けられている。支持部１２１−ｉの長手方向の一端側の内壁面１２１ａ−ｉには、ばね１２２−ｉの一端が固定され、ばね１２２−ｉの他端は永久磁石１２５−ｉの端部１２５ａ−ｉに固定されている。また、支持部１２１−ｉの長手方向の他端側の内壁面１２１ｂ−ｉには、ばね１２３−ｉの一端が固定され、ばね１２３−ｉの他端は永久磁石１２５−ｉの端部１２５ｂ−ｉに固定されている。

支持部１２１−ｉの外周側にはコイル１２４−ｉが巻きつけられている。ただし、永久磁石１２５−ｉの端部１２５ａ−ｉ側（Ｎ極側）では、第１巻き部１２４ａ−ｉがＡ_１方向に巻きつけられており、端部１２５ｂ−ｉ側（Ｓ極側）では、第２巻き部１２４ｂ−ｉがＡ_１方向（奥から手前に向けた方向）と反対向きのＢ_１方向（手前から奥に向けた方向）に巻き付けられている。すなわち、永久磁石１２５−ｉの端部１２５ａ−ｉ側（Ｎ極側）からみた場合、第１巻き部１２４ａ−ｉは時計回りに巻き付けられており、第２巻き部１２４ｂ−ｉは反時計回りに巻き付けられている。また、永久磁石１２５−ｉが停止し、ばね１２２−ｉ，１２３−ｉからの弾性力が釣り合った状態において、永久磁石１２５−ｉの端部１２５ａ−ｉ側（Ｎ極側）が第１巻き部１２４ａ−ｉの領域に配置され、端部１２５ｂ−ｉ側（Ｓ極側）が第２巻き部１２４ｂ−ｉの領域に配置されることが望ましい。

以上のように配置構成された支持部１２１−ｉ、ばね１２２−ｉ，１２３−ｉ、コイル１２４−ｉ、および永久磁石１２５−ｉが、ケース１２６−ｉ内に収容され、支持部１２１−ｉがケース１２６−ｉの内部に固定されている。ただし、ケース１２６−ｉの長手方向は、支持部１２１−ｉの長手方向および永久磁石１２５−ｉの長手方向と一致する。

コイル１２４ａ−ｉに奥から手前に向かうＡ_１方向（コイル１２４ｂ−ｉではＢ_１方向）に電流を流すと、フレミングの左手の法則で説明されるローレンツ力の反作用により、永久磁石１２５−ｉにＣ_１方向（永久磁石１２５−ｉのＮ極からＳ極に向かう方向：右方向）の力が加えられる（図５Ａ）。逆に、コイル１２４−ｉに手前から奥に向かうＡ_２方向（Ｂ_２方向）に電流を流すと、永久磁石１２５−ｉにＣ_２方向（永久磁石１２５−ｉのＳ極からＮ極に向かう方向：左方向）の力が加えられる（図５Ｂ）。ただし、Ａ_２方向はＡ_１方向の反対方向である。これらの動作により、永久磁石１２５−ｉおよびばね１２２−ｉ，１２３−ｉからなる系に運動エネルギーが与えられる。それにより、ケース１２６−ｉを基準とする永久磁石１２５−ｉの位置および加速度を変化させることができる。

ここで、コイル１２４−ｉに所定の方向に電流を流す期間（時間）とそれ以外の期間とを周期的に繰り返す。その際、所定の方向に電流を流す期間とそれ以外の期間との比（反転比）を何れか一方の期間に偏らせることにより、所望の方向に擬似的な力覚を提示できる。Ａ_１方向（Ｂ_１方向）の電流（Ｘ）を流す期間（時間）ｔ_１とＡ_２方向（Ｂ_２方向）の電流（−Ｘ）を流す期間ｔ_２とを周期的に繰り返す。この場合、Ａ_１方向（Ｂ_１方向）の電流を流す期間ｔ_１とＡ_２方向（Ｂ_２方向）の電流を流す期間ｔ_２との比（反転比ｔ_１：ｔ_２）に応じ、図５Ａおよび図５Ｂの左方向または右方向に擬似的な力覚を提示できる。すなわち、図５Ａおよび図５Ｂの左方向に擬似的な力覚を提示する場合には、ｔ_１＞ｔ_２となる反転比の周期的な電流をコイル１２４−ｉに流す。例えば、反転比ｔ_１：ｔ_２＝１８ｍｓｅｃ：７ｍｓｅｃの周期的な電流（４０Ｈｚの周波数の電流）をコイル１２４−ｉに流す。逆に、右方向に擬似的な力覚を提示する場合には、ｔ_１＜ｔ_２となる反転比の周期的な電流をコイル１２４−ｉに流す。例えば、反転比ｔ_１：ｔ_２＝７ｍｓｅｃ：１８ｍｓｅｃの周期的な電流（４０Ｈｚの周波数の電流）をコイル１２４−ｉに流す。

＜撮影装置１３＞
撮影装置１３は周辺画像を得るカメラ等の装置である。本実施形態では、カラーの周辺画像を撮影可能な撮影装置１３を用いる。撮影装置１３は、位置推定装置１１の基準点に対する相対位置が固定されている撮影位置に設置されている。本実施形態では撮影位置を基準点とする。図６に加速度発生装置１２と撮影装置１３を一体化した構成を例示する。図６の例では、例えば、加速度発生装置１２の重心または重心の近傍が撮影装置１３の撮影位置となっている。

＜事前設定＞
位置推定装置１１と加速度発生装置１２と撮影装置１３とマーカ１４−１〜１４−Ｎは、実空間内に配置されている。このうち、マーカ１４−１〜１４−Ｎは、実空間内の互いに異なる位置（例えば、天井や壁など）に固定されている。互いに異なるマーカ１４−ｉ（ただし、ｉ＝１，・・・，Ｎ）は互いに異なる平面上に配置されていてもよいが、個々のマーカ１４−ｉ全体は同一平面上に配置される（個々のマーカ１４−ｉは折れ曲がったり、曲がった面に配置されたりせず、各平面上に配置される）。

一方、位置推定装置１１と加速度発生装置１２と撮影装置１３の実空間内の位置は固定されていないが、加速度発生装置１２と撮影装置１３との間の相対位置は固定されている。位置推定装置１１と加速度発生装置１２と撮影装置１３がこの実空間内を移動すると、撮影装置１３で撮影される周辺画像が変化し、そこに写り込むマーカ１４−ｉ（ただし、ｉ＝１，・・・，Ｎ）も変化する。周辺画像にすべてのマーカ１４−ｉの画像が含まれる必要はなく、マーカ１４−１〜１４−Ｎの一部の画像のみが含まれてもよい。ただし、周辺画像が複数個のマーカ１４−ｉの画像を含むことで高精度で位置検出を行うことができる。周辺画像に６個以上のマーカ１４−ｉの画像が含まれることが望ましい。周辺画像に含まれるマーカ１４−ｉの画像の個数は、マーカ１４−１〜１４−Ｎと撮影装置１３との距離、マーカ１４−１〜１４−Ｎのサイズや配置間隔に依存する。例えば、マーカ１４−１〜１４−Ｎが固定される天井等の平面と撮影装置１３との距離が２ｍのとき、一辺が１５０ｍｍのマーカ１４−１〜１４−Ｎをそれらの中心間隔がそれぞれ５００ｍｍとなるように配置する。

また、マーカ１４−ｉ（ただし、ｉ＝１，・・・，Ｎ）の世界座標系における中心座標（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）とマーカ幅ｗ_ｉが事前に得られ、マーカ１４−ｉの識別子ＩＤ_ｉと世界座標系における中心座標（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）とマーカ幅ｗ_ｉとを対応付けたデータベースが位置推定装置１１の記憶部１１１に格納される。なお、実施形態では、世界座標系における座標（Ｘ，Ｙ，Ｚ）のＸ座標およびＹ座標は水平成分の座標を表し、Ｚ座標は鉛直成分の座標を表すことにする。以下に、このデータベースを例示する。

＜処理＞
図７を用いて本実施形態の処理を説明する。
《撮影》
撮影装置１３が周囲を撮影し、それによってカラーの周辺画像（図３Ａ）を得る（ステップＳ１０１）。周辺画像はマーカ位置取得部１１３に送られる。

《マーカの位置情報取得》
マーカ位置取得部１１３は、この周辺画像に由来する複数個のマーカ１４−ｉの画像情報に基づいて複数個のマーカ１４−ｉの位置情報を得る。本実施形態では、（１）マーカ１４−ｉの画像情報から得られるマーカ１４−ｉの各頂点の画像座標系における座標（マーカの画像情報から特定されるマーカの画像座標系における座標）、および、（２）マーカ１４−ｉの画像情報から得られる識別子ＩＤ_ｉを記憶部１１１のデータベースに照合して得られるマーカ１４−ｉの世界座標系における座標（マーカの世界座標系における座標を特定するための情報から特定されるマーカの世界座標系における座標）をマーカ１４−ｉの位置情報とする。以下にこの処理を詳細に説明する。

まず、マーカ位置取得部１１３は、送られたカラーの周辺画像を二値化し、二値化した周辺画像（図３Ｂ）を得る。なお、前述の図２Ｃのように白黒を反転させたマーカ１４−ｉを用いる場合にも、周辺画像の二値化し、白黒を反転させる（図３Ｂ）。次に、マーカ位置取得部１１３は、二値化した周辺画像を解析し、黒色のピクセルからなる領域の輪郭（図３Ｃ）を抽出する。このように抽出された輪郭に囲まれた領域はマーカ１４−ｉを含む可能性が高い。マーカ位置取得部１１３は、抽出された輪郭に対して多角形近似を行って多角形を抽出する（ステップＳ１０２）。

マーカ位置取得部１１３は、抽出した多角形が予め設定しておいたマーカ１４−ｉの輪郭に適合するかを判定する。ここで「適合する」とは、抽出した多角形の特徴量と予め設定しておいたマーカ１４−ｉの輪郭の特徴量との類似度が閾値以上であること、またはこれらの距離が閾値以下であることである。ここで、適合する多角形が存在しない場合にはステップＳ１０１に戻る。一方、適合する多角形が存在する場合には、適合する多角形のみをマーカ候補（マーカの画像情報）として記憶部１１１に格納する（ステップＳ１０３）。

マーカ位置取得部１１３は、記憶部１１１からステップＳ１０５以降の処理がなされていないマーカ候補を抽出する。マーカ位置取得部１１３は、抽出したマーカ候補に対してどの角がマーカの外枠の切り欠き１４１に相当するかを決定する（図３Ｄ）。マーカ位置取得部１１３は、決定した切り欠き１４１に基づいてマーカ候補の正しい向きに補正する。例えば、マーカ位置取得部１１３は、切り欠き１４１に基づいてマーカ候補の各頂点をそのマーカ候補に対応するマーカの各頂点に射影するホモグラフィー行列を求め、それに基づいてマーカ候補を正しい向きに射影する。その後、マーカ位置取得部１１３は、補正されたマーカ候補の特定の領域１４２から識別子ＩＤ_ｉを抽出する。例えば、特定の領域１４２内の白および黒のピクセルの比率を算出し、識別子ＩＤ_ｉを抽出する。さらに、マーカ位置取得部１１３は、補正されたマーカ候補の他の領域１４３内の図形から誤り検出ビットを抽出し、それを用いて識別子ＩＤ_ｉをチェックしてもよい（ステップＳ１０４）。

マーカ位置取得部１１３は、得られた識別子ＩＤ_ｉが有効であるか否かを判定する。例えば、識別子ＩＤ_ｉが予め定められた範囲に属しない場合や誤り検出ビットによるチェックがエラーである場合には得られた識別子ＩＤ_ｉが有効でないと判定し、それ以外の場合に有効であると判定する。得られた識別子ＩＤ_ｉが有効でないと判定した場合にはステップＳ１１０に進み、得られた識別子ＩＤ_ｉが有効であると判定した場合にはステップＳ１０６に進む（ステップＳ１０５）。

ステップＳ１０６では、マーカ位置取得部１１３は、マーカ候補の各頂点ｍ（ｉ）（例えば、図３Ｆで白丸で示した頂点）の画像座標系における座標（_１ｕ_ｉ，_１ｖ_ｉ）〜（_Ｍ（ｉ）ｕ_ｉ，_Ｍ（ｉ）ｖ_ｉ）を取得する。ただし、ｍ（ｉ）＝１，・・・，Ｍ（ｉ）であり、Ｍ（ｉ）はマーカ候補の頂点の個数を表す正整数である（ステップＳ１０６）。マーカ位置取得部１１３は、さらに記憶部１１１に格納されたデータベースを参照し、マーカ候補から得られた識別子ＩＤ_ｉに対応するマーカ１４−ｉの世界座標系における中心座標（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）を取得する（ステップＳ１０７）。マーカ位置取得部１１３は、それら抽出した世界座標系における中心座標（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）を用い、マーカ１４−ｉの各頂点ｍ（ｉ）＝１，・・・，Ｍ（ｉ）の世界座標系における既知の座標（_１ｘ_ｉ，_１ｙ_ｉ，_１ｚ_ｉ）〜（_Ｍ（ｉ）ｘ_ｉ，_Ｍ（ｉ）ｙ_ｉ，_Ｍ（ｉ）ｚ_ｉ）を得る。例えば、マーカ位置取得部１１３は、各マーカ１４−ｉの中心座標（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）に対する各頂点の相対位置の情報を保持しているか、あるいはマーカ幅ｗ_ｉを使ってそれを計算により求めることができる。マーカ位置取得部１１３は、それを用いて入力された中心座標（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）から各頂点ｍ（ｉ）の既知の座標（_１ｘ_ｉ，_１ｙ_ｉ，_１ｚ_ｉ）〜（_Ｍ（ｉ）ｘ_ｉ，_Ｍ（ｉ）ｙ_ｉ，_Ｍ（ｉ）ｚ_ｉ）を得る（ステップＳ１０８）。マーカ位置取得部１１３は、このように得られたマーカ候補の各頂点ｍ（ｉ）（ただし、ｍ（ｉ）＝１，・・・，Ｍ（ｉ））の画像座標系における座標（_１ｕ_ｉ，_１ｖ_ｉ）〜（_Ｍ（ｉ）ｕ_ｉ，_Ｍ（ｉ）ｖ_ｉ）、および世界座標系における既知の座標（_１ｘ_ｉ，_１ｙ_ｉ，_１ｚ_ｉ）〜（_Ｍ（ｉ）ｘ_ｉ，_Ｍ（ｉ）ｙ_ｉ，_Ｍ（ｉ）ｚ_ｉ）を記憶部１１１に格納する（ステップＳ１０９）。

マーカ位置取得部１１３は、ステップＳ１０５以降の処理を行っていないマーカ候補が存在するか判定する（ステップＳ１１０）。ここで、ステップＳ１０５以降の処理を行っていないマーカ候補が存在する場合にはステップＳ１０４に戻り、存在しない場合にはステップＳ１１１に進む。

《位置推定処理》
上述のように得られた複数個のマーカ候補の各頂点ｍ（ｉ）（ただし、ｍ（ｉ）＝１，・・・，Ｍ（ｉ））の画像座標系における座標（_１ｕ_ｉ，_１ｖ_ｉ）〜（_Ｍ（ｉ）ｕ_ｉ，_Ｍ（ｉ）ｖ_ｉ）、および世界座標系にける既知の座標（_１ｘ_ｉ，_１ｙ_ｉ，_１ｚ_ｉ）〜（_Ｍ（ｉ）ｘ_ｉ，_Ｍ（ｉ）ｙ_ｉ，_Ｍ（ｉ）ｚ_ｉ）は、位置推定部１１４に送られる。位置推定部１１４は、これらを用いて座標変換行列を得、撮影装置１３の位置（基準点）の位置推定値を得る。すなわち、位置推定部１１４は、ステップＳ１０３およびＳ１０５でｙｅｓとされた有効なマーカ候補の頂点ｍ（ｉ）の画像座標系における２次元座標（ｕ，ｖ）に対してＰｎＰ（perspective n-points）問題を解き、それによって得られる座標変換行列から撮影装置１３の位置を推定する。以下にこの詳細を説明する。

マーカの頂点の画像座標系における座標（ｕ，ｖ）、頂点の既知の世界座標系における座標（ｘ，ｙ，ｚ）、既知のカメラパラメータの行列Ｃ（焦点距離ｆ_ｘ，ｆ_ｙ、画像中心ｃ_ｘ，ｃ_ｙ）、および既知のスケールパラメータｓには以下の関係が成り立つ。

ただし、

である。Ｒは回転操作を表す行列であり、ｔは並進移動操作を表す行列であり、［Ｒ｜ｔ］を座標変換行列と呼ぶ。

世界座標系における撮影装置１３の位置ｃは式（１）のｔとＲを用いて、ｔ＝−Ｒｃの関係が成立する。すなわち、世界座標系における撮影装置１３の位置ｃは以下のように算出できる。
ｃ＝−Ｒ^−１ｔ （２）
ただし、Ｒ^−１はＲの逆行列である。一方、ｄをカメラ座標系における点の位置、カメラ座標系における点の位置ｄに対応する世界座標系における点の位置（既知）をａとしたとき、ｄ＝Ｒ（ａ−ｃ）が成り立つ。よって以下が成り立つ。
ｃ＝ａ−Ｒ^−１ｄ （３）
なお、「カメラ座標系」とは、カメラに対して相対位置が固定された点を原点とした３次元座標系のことであり、この座標系の原点は、世界座標系においては撮影装置１３の位置に相当する。

また、撮影装置１３によって撮影された周辺画像から得られたマーカの頂点のカメラ座標系における座標を（Ｌｘ，Ｌｙ，Ｌｚ）とすると以下の関係が成り立つ。

式（４）を式（１）に代入すると以下が成り立つ。

従って、周辺画像からカメラ座標系における頂点の座標（Ｌｘ，Ｌｙ，Ｌｚ）として合計ｎ個の座標（Ｌｘ_１，Ｌｙ_１，Ｌｚ_１），・・・，（Ｌｘ_ｎ，Ｌｙ_ｎ，Ｌｚ_ｎ）が得られており、それらに対応する既知の世界座標系における頂点の座標が（ｘ_１，ｙ_１，ｚ_１），・・・，（ｘ_ｎ，ｙ_ｎ，ｚ_ｎ）である場合、以下が成り立つ。

ここで、式（６）の第２項の（ｘ_１，ｙ_１，ｚ_１），・・・，（ｘ_ｎ，ｙ_ｎ，ｚ_ｎ）を列ベクトルとする行列Ｙの擬似逆行列［Ｙ］^＋を求め、それを式（６）の両辺に掛けると以下が成り立つ。

従って、周辺画像から得られたカメラ座標系における頂点の座標（Ｌｘ_１，Ｌｙ_１，Ｌｚ_１），・・・，（Ｌｘ_ｎ，Ｌｙ_ｎ，Ｌｚ_ｎ）と、それらに対応する世界座標系における頂点の座標（既知）（ｘ_１，ｙ_１，ｚ_１），・・・，（ｘ_ｎ，ｙ_ｎ，ｚ_ｎ）とを用い、式（７）を計算することで座標変換行列［Ｒ｜ｔ］を求めることができる。さらに得られた座標変換行列［Ｒ｜ｔ］の行列Ｒおよびｔを式（２）に代入することで世界座標系における撮影装置１３の位置ｃを計算することができる。このとき、この計算に用いることができるマーカの頂点の総数ｎが大きいほど、つまり多くのマーカが撮影できればできるほど、世界座標系における撮影装置１３の位置ｃの計測誤差が小さくなることが期待できる。

以上に基づき、位置推定部１１４の処理手順を説明する。位置推定部１１４は、送られたすべてのマーカ１４−ｉの各頂点ｍ（ｉ）（ただし、ｍ（ｉ）＝１，・・・，Ｍ（ｉ））の画像座標系における座標（ｕ，ｖ）＝（_１ｕ_ｉ，_１ｖ_ｉ），・・・，（_Ｍ（ｉ）ｕ_ｉ，_Ｍ（ｉ）ｖ_ｉ）を式（４）に代入し、各マーカ１４−ｉの各頂点のカメラ座標系における既知の座標（Ｌｘ，Ｌｙ，Ｌｚ）＝（_１Ｌｘ_ｉ，_１Ｌｙ_ｉ，_１Ｌｚ_ｉ），・・・，（_Ｍ（ｉ）Ｌｘ_ｉ，_Ｍ（ｉ）Ｌｙ_ｉ，_Ｍ（ｉ）Ｌｚ_ｉ）を得る。次に、位置推定部１１４は、得られたすべてのマーカ１４−ｉのカメラ座標系における頂点の座標（_１Ｌｘ_ｉ，_１Ｌｙ_ｉ，_１Ｌｚ_ｉ）〜（_Ｍ（ｉ）Ｌｘ_ｉ，_Ｍ（ｉ）Ｌｙ_ｉ，_Ｍ（ｉ）Ｌｚ_ｉ）を（Ｌｘ_１，Ｌｙ_１，Ｌｚ_１），・・・，（Ｌｘ_ｎ，Ｌｙ_ｎ，Ｌｚ_ｎ）とし、それらの世界座標系における既知の座標（_１ｘ_ｉ，_１ｙ_ｉ，_１ｚ_ｉ）〜（_Ｍ（ｉ）ｘ_ｉ，_Ｍ（ｉ）ｙ_ｉ，_Ｍ（ｉ）ｚ_ｉ）を（ｘ_１，ｙ_１，ｚ_１），・・・，（ｘ_ｎ，ｙ_ｎ，ｚ_ｎ）とし、式（７）によって座標変換行列［Ｒ｜ｔ］を求める。すなわち、位置推定部１１４に送られた頂点の総数をｎとする。次に、位置推定部１１４は、得られた座標変換行列［Ｒ｜ｔ］の行列Ｒおよびｔを式（２）に代入することで撮影装置１３の世界座標系における位置ｃ＝（Ｘ_ｃ，Ｙ_ｃ，Ｚ_ｃ）（位置推定値）を計算する（ステップＳ１１１）。

《方位推定処理》
目的位置生成部１１２は、位置推定値ｃ＝（Ｘ_ｃ，Ｙ_ｃ，Ｚ_ｃ）を入力とし、実空間内の次の目的位置（経由ノード）の世界座標系における座標ｄ＝（Ｘ_ｄ，Ｙ_ｄ，０）を生成して出力する。目的位置生成部１１２は、位置推定値ｃが表す位置から最終目的位置（事前設定された位置）までの経路を設定し、その経路における経由ノードの世界座標系における座標ｄ＝（Ｘ_ｄ，Ｙ_ｄ，０）を自動的に生成して出力する。

位置推定部１１４で得られた位置推定値ｃ＝（Ｘ_ｃ，Ｙ_ｃ，Ｚ_ｃ）および目的位置生成部１１２で得られた座標ｄ＝（Ｘ_ｄ，Ｙ_ｄ，０）は、方位推定部１１５に入力される。方位推定部１１５は、これらを用い、位置（Ｘ_ｃ，Ｙ_ｃ，０）（基準点または基準点に対する所定の相対位置）を基点とした次の目的位置（Ｘ_ｄ，Ｙ_ｄ，０）の方向を推定し、その方向を表す方位推定値Θｚを出力する。例えば、方位推定部１１５は、以下のように方位推定値Θｚを得て出力する。
Θｚ＝ａｒｃｔａｎ（（Ｙ_ｄ−Ｙ_ｃ）／（Ｘ_ｄ−Ｘ_ｃ））−ψｚ （８）
ただし、ψｚは、Θｚ＝０として加速度発生装置１２を動かした場合の、加速度発生装置１２が擬似的な力覚を生成する方向と世界座標系のＸ軸がなす角度を表す。式（８）で得られる方位推定値Θｚは、この進行方向を基準とした次の目的位置（Ｘ_ｄ，Ｙ_ｄ，０）の相対的な方向を表す。例えば、Θｚ＝０はこの進行方向に次の目的位置（Ｘ_ｄ，Ｙ_ｄ，０）があることを表す。

方位推定値Θｚは、位置推定値ｃ＝（Ｘ_ｃ，Ｙ_ｃ，Ｚ_ｃ）が新たに方位推定部１１５に入力されるたびに再計算される。これにより、利用者の位置や向きの変化に応じ、方位推定値Θｚがリアルタイムにアップデートされる。これにより、利用者が経路から外れた場合であっても、利用者を経路に戻すための方向を刻々と修正して提示することができる。座標（Ｘ_ｃ，Ｙ_ｃ，０）が座標（Ｘ_ｄ，Ｙ_ｄ，０）の近傍に到達すると、目的位置生成部１１２は次の目的位置（経由ノード）の座標（Ｘ_ｃ，Ｙ_ｃ，０）を新たに得て出力する。方位推定部１１５は、これと位置推定値ｃ＝（Ｘ_ｃ，Ｙ_ｃ，Ｚ_ｃ）とを用いて同様に方位推定値Θｚを生成して出力する。これを終点目的位置まで繰り返すことにより、経路誘導を実現するための方位推定値Θｚを生成していく（ステップＳ１１２）。

《制御情報の生成》
方位推定値Θｚは制御部１１６に入力される。制御部１１６は、方位推定値Θｚが表す目的位置の方向成分を持つ向きに、加速度発生装置１２が振動による方向提示を行うための制御情報を生成して出力する。例えば、図４Ａおよび図４Ｂに例示したような加速度発生装置１２の場合、制御部１１６は、振動子１２０−１を方位推定値Θｚが表す目的位置の方向成分を持つ向き（例えば、方位推定値Θｚが表す方向）に回転させるための制御情報と、この方向成分を持つ方向（例えば、方位推定値Θｚが表す方向）の加速度の時間変化とその逆方向の加速度の時間変化とが非対称な偏加速度運動を振動子１２０−１に行わせるための制御情報とを生成して出力する。例えば、図４Ｃに例示したような加速度発生装置１２の場合、制御部１１６は、振動子１２０−１から１２０−４が提示する擬似的な力覚の方向の合成ベクトルが方位推定値Θｚが表す目的位置の方向成分を持つ向き（例えば、方位推定値Θｚが表す方向）となるように、振動子１２０−１から１２０−４を制御するための制御情報を生成して出力する（ステップＳ１１３）。加速度発生装置１２は、入力された制御情報に基づいて駆動して擬似的な力覚を提示する。これにより、加速度発生装置１２を把持する利用者を経路誘導できる。

制御部１１６は処理を終了するか否かを判定する。例えば、制御部１１６は、座標（Ｘ_ｃ，Ｙ_ｃ，０）が最終的な目的値の座標（Ｘ_ｄ，Ｙ_ｄ，０）の近傍に到達した場合や、所定の時間が経過した場合などに処理を終了すると判定する。制御部１１６が処理を終了すると判定しない場合にはステップＳ１０１に戻る。制御部１１６が処理を終了すると判定する場合には処理を終了する。

［第２実施形態］
本実施形態は第１実施形態の変形例である。本実施形態の周辺画像は、複数の平面のそれぞれに位置するマーカの画像を含み、位置推定部は、同一の平面に位置する複数個のマーカの画像に由来する複数個のマーカの画像情報に基づいて基準点の位置の予備推定値を得、平面のそれぞれに対して得られた予備推定値を用いて位置推定値を得る。以下では、第１実施形態との相違点を中心に説明し、第１実施形態と共通する事項については同じ参照番号を用いて説明を省略する。

図１に例示するように、本形態の位置推定システム２は、位置推定装置２１と加速度発生装置１２と撮影装置１３と複数個のマーカ２４−１〜２４−Ｎとを有する。ただし、Ｎは２以上の整数である。

＜マーカ２４−１〜２４−Ｎ＞
本実施形態のマーカ２４−ｉ（ただし、ｉ＝１，・・・，Ｎ）は、マーカ２４−ｉを表す識別子ＩＤ_ｉだけではなく、マーカ２４−ｉが設置される平面（例えば、天井や壁等）を表す平面識別子ＰＩＤ_ｉをも表している。その他、マーカ２４−ｉが識別子ＩＤ_ｉや平面識別子ＰＩＤ_ｉの誤り検出ビットを表していてもよい。その他は第１実施形態のマーカ１４−１〜１４−Ｎと同じである。

＜位置推定装置２１＞
本実施形態の位置推定装置２１は、記憶部２１１と目的位置生成部１１２とマーカ位置取得部１１３と位置推定部２１４と方位推定部１１５と制御部１１６とを有する。位置推定装置２１は、例えば、前述のコンピュータが所定のプログラムを実行することで構成される装置である。また、単独で処理機能を実現する電子回路を用いて一部またはすべての処理部が構成されてもよい。

＜加速度発生装置１２・撮影装置１３＞
第１実施形態と同じである。

＜事前設定＞
位置推定装置２１と加速度発生装置１２と撮影装置１３とマーカ２４−１〜２４−Ｎは、実空間内に配置されている。このうち、マーカ２４−１〜２４−Ｎは、実空間内の互いに異なる位置に固定されている。互いに異なるマーカ２４−ｉ（ただし、ｉ＝１，・・・，Ｎ）は互いに異なる平面上に配置されていてもよいが、個々のマーカ２４−ｉ全体は同一平面上に配置される（個々のマーカ１４−ｉは折れ曲がったり、曲がった面に配置されたりせず、各平面上に配置される）。一方、位置推定装置２１と加速度発生装置１２と撮影装置１３の実空間内の位置（世界座標系における位置）は固定されていない（すなわち、これらは実空間内を移動可能である）が、加速度発生装置１２と撮影装置１３との間の相対位置は固定されている。

また、マーカ２４−ｉ（ただし、ｉ＝１，・・・，Ｎ）の世界座標系における中心座標（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）とマーカ２４−ｉが配置される平面の情報とマーカ幅ｗ_ｉが事前に得られ、マーカ２４−ｉの識別子ＩＤ_ｉと世界座標系における中心座標（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）とマーカ幅ｗ_ｉとマーカ２４−ｉが配置される平面（例えば、天井や壁など）の平面識別子ＰＩＤ_ｉとを対応付けたデータベースが位置推定装置２１の記憶部２１１に格納される。以下に、このデータベースを例示する。

＜処理＞
図７および図８を用いて本実施形態の処理を説明する。
《撮影・マーカの位置情報取得》
ステップＳ１０１からＳ１１０までの処理は、マーカ１４−ｉがマーカ２４−ｉに置換される以外、第１実施形態と同じである。ただし、撮影された周辺画像は、複数の平面のそれぞれに位置するマーカの画像を含み得る。

《位置推定処理》
位置推定部２１４には、マーカ位置取得部１１３で得られた、複数個のマーカ候補の各頂点ｍ（ｉ）（ただし、ｍ（ｉ）＝１，・・・，Ｍ（ｉ））の画像座標系における座標（_１ｕ_ｉ，_１ｖ_ｉ）〜（_Ｍ（ｉ）ｕ_ｉ，_Ｍ（ｉ）ｖ_ｉ）、世界座標系における既知の座標（_１ｘ_ｉ，_１ｙ_ｉ，_１ｚ_ｉ）〜（_Ｍ（ｉ）ｘ_ｉ，_Ｍ（ｉ）ｙ_ｉ，_Ｍ（ｉ）ｚ_ｉ）、およびマーカ候補に対応するマーカ２４−ｉの識別子ＩＤ_ｉが入力される。さらに位置推定部２１４は、記憶部２１１に格納されたデータベースを参照し、識別子ＩＤ_ｉに対応する平面識別子ＰＩＤ_ｉを抽出する。

位置推定部２１４は、これらを用いて、同一の平面に位置する複数個のマーカの画像に由来する複数個のマーカの画像情報に基づいて撮影装置１３（基準点）の位置の予備推定値を得、平面のそれぞれに対して得られた予備推定値を用いて位置推定値を得る（ステップＳ２１１）。以下にこの詳細を説明する。

まず、位置推定部２１４は、ステップＳ２１１２以降の処理が実行されていない平面識別子ＰＩＤ_ｉを選択し、それが有効なものであるかを判定する。例えば、平面識別子ＰＩＤ_ｉが予め定められた範囲に属しない場合や誤り検出ビットによるチェックがエラーである場合には得られた平面識別子ＰＩＤ_ｉが有効でないと判定し、それ以外の場合に有効であると判定する。選択した平面識別子ＰＩＤ_ｉが有効でないと判定した場合にはステップＳ２１１４に進み、選択した平面識別子ＰＩＤ_ｉが有効であると判定した場合にはステップＳ２１１２に進む（ステップＳ２１１１）。

ステップＳ２１１２では、位置推定部２１４は、選択した平面識別子ＰＩＤ_ｉに対応するマーカ２４−ｉ（ただし、ｍ（ｉ）＝１，・・・，Ｍ（ｉ））の画像座標系における座標（ｕ，ｖ）＝（_１ｕ_ｉ，_１ｖ_ｉ），・・・，（_Ｍ（ｉ）ｕ_ｉ，_Ｍ（ｉ）ｖ_ｉ）を式（４）に代入し、各マーカ２４−ｉの各頂点のカメラ座標系における座標（Ｌｘ，Ｌｙ，Ｌｚ）＝（_１Ｌｘ_ｉ，_１Ｌｙ_ｉ，_１Ｌｚ_ｉ），・・・，（_Ｍ（ｉ）Ｌｘ_ｉ，_Ｍ（ｉ）Ｌｙ_ｉ，_Ｍ（ｉ）Ｌｚ_ｉ）を得る。次に、位置推定部２１４は、選択した平面識別子ＰＩＤ_ｉに対応するカメラ座標系における座標（_１Ｌｘ_ｉ，_１Ｌｙ_ｉ，_１Ｌｚ_ｉ）〜（_Ｍ（ｉ）Ｌｘ_ｉ，_Ｍ（ｉ）Ｌｙ_ｉ，_Ｍ（ｉ）Ｌｚ_ｉ）を（Ｌｘ_１，Ｌｙ_１，Ｌｚ_１），・・・，（Ｌｘ_ｎ，Ｌｙ_ｎ，Ｌｚ_ｎ）とし、それらの世界座標系における座標（_１ｘ_ｉ，_１ｙ_ｉ，_１ｚ_ｉ）〜（_Ｍ（ｉ）ｘ_ｉ，_Ｍ（ｉ）ｙ_ｉ，_Ｍ（ｉ）ｚ_ｉ）を（ｘ_１，ｙ_１，ｚ_１），・・・，（ｘ_ｎ，ｙ_ｎ，ｚ_ｎ）とし、式（７）によって座標変換行列［Ｒ｜ｔ］を求め、行列Ｒおよびｔを式（２）に代入することで撮影装置１３の世界座標系における位置ｃ’（ＰＩＤ_ｉ）＝（Ｘ_ｃ’，Ｙ_ｃ’，Ｚ_ｃ’）（予備推定値）を計算する（ステップＳ２１１２）。位置推定部２１４は、位置ｃ’（ＰＩＤ_ｉ）を平面識別子ＰＩＤ_ｉに対応付けて記憶部２１１に格納する（ステップＳ２１１３）。

位置推定部２１４は、すべての平面識別子ＰＩＤ_ｉに対応する処理が終了したかを判定する（ステップＳ２１１４）。ここで、すべての平面識別子ＰＩＤ_ｉに対応する処理が終了していないと判定された場合にはステップＳ２１１１に戻る。一方、すべての平面識別子ＰＩＤ_ｉに対応する処理が終了したと判定された場合にはステップＳ２１１５に進む。

ステップＳ２１１５では、位置推定部２１４が、各平面識別子ＰＩＤ_ｉに対応するグループでそれぞれ得られた位置ｃ’（ＰＩＤ_ｉ）を用いて撮影装置１３の世界座標系における位置ｃ＝（Ｘ_ｃ，Ｙ_ｃ，Ｚ_ｃ）（位置推定値）を計算する。例えば、位置推定部２１４は、グループでそれぞれ得られた位置ｃ’（ＰＩＤ_ｉ）を平均したものを位置ｃとしてもよいし、推定精度が最も高い（計測誤差の最も小さい）位置ｃ’（ＰＩＤ_ｉ）を位置ｃとしてもよい。推定精度の評価方法には様々なものが考えられる。例えば、式（７）の計算においてｎが大きいほど推定精度が高くなる傾向がある。そのため、最も多くの座標（_１Ｌｘ_ｉ，_１Ｌｙ_ｉ，_１Ｌｚ_ｉ）〜（_Ｍ（ｉ）Ｌｘ_ｉ，_Ｍ（ｉ）Ｌｙ_ｉ，_Ｍ（ｉ）Ｌｚ_ｉ）が属する平面の平面識別子ＰＩＤ_ｉに対応する位置ｃ’（ＰＩＤ_ｉ）を位置ｃとしてもよい。或いは、最小二乗法によって推定精度を計算してもよい。例えば、位置推定部２１４は、平面識別子ＰＩＤ_ｉに対応する式（７）の［Ｒ｜ｔ］をＡとおき、その擬似逆行列をＡ^＋とし、

の二乗ノルムを計算する。ただし、平面識別子ＰＩＤ_ｉに対応するカメラ座標系における座標（_１Ｌｘ_ｉ，_１Ｌｙ_ｉ，_１Ｌｚ_ｉ）〜（_Ｍ（ｉ）Ｌｘ_ｉ，_Ｍ（ｉ）Ｌｙ_ｉ，_Ｍ（ｉ）Ｌｚ_ｉ）を（Ｌｘ_１，Ｌｙ_１，Ｌｚ_１），・・・，（Ｌｘ_ｎ，Ｌｙ_ｎ，Ｌｚ_ｎ）とし、世界座標系における座標（_１ｘ_ｉ，_１ｙ_ｉ，_１ｚ_ｉ）〜（_Ｍ（ｉ）ｘ_ｉ，_Ｍ（ｉ）ｙ_ｉ，_Ｍ（ｉ）ｚ_ｉ）を（ｘ_１，ｙ_１，ｚ_１），・・・，（ｘ_ｎ，ｙ_ｎ，ｚ_ｎ）とする。また、Ａ⁺＝（Ａ^ＴＡ）^−１Ａ^Ｔである。・^Ｔは・の転置を示す。このＥの二乗ノルムが最も小さいほど推定精度が高い。そのため、位置推定部２１４は、Ｅの二乗ノルムが最も小さくなる平面識別子ＰＩＤ_ｉに対応する位置ｃ’（ＰＩＤ_ｉ）を位置ｃとしてもよい（ステップＳ２１１５）。その他の処理は第１実施形態と同じである。

［第３実施形態］
第３実施形態は第１実施形態の変形例であり、位置推定の精度が高いほど加速度発生装置が明確な振動刺激を提示する。以下では、第１実施形態との相違点を中心に説明し、第１実施形態と共通する事項については同じ参照番号を用いて説明を省略する。

図１に例示するように、本形態の位置推定システム３は、位置推定装置３１と加速度発生装置１２と撮影装置１３と複数個のマーカ１４−１〜１４−Ｎとを有する。

＜マーカ１４−１〜１４−Ｎ＞
第１実施形態と同じである。

＜位置推定装置１１＞
本実施形態の位置推定装置３１は、記憶部１１１と目的位置生成部１１２とマーカ位置取得部１１３と位置推定部３１４と方位推定部１１５と制御部３１６とを有する。位置推定装置３１は、例えば、前述のコンピュータが所定のプログラムを実行することで構成される装置である。或いは、単独で処理機能を実現する電子回路を用いて一部またはすべての処理部が構成されてもよい。

＜加速度発生装置１２・撮影装置１３＞
第１実施形態と同じである。

＜事前設定＞
第１実施形態と同じである。

＜処理＞
図７を用いて本実施形態の処理を説明する。
《撮影・マーカの位置情報取得》
ステップＳ１０１からＳ１１０までの処理は第１実施形態と同じである。

《位置推定処理》
位置推定部３１４には、マーカ位置取得部１１３で得られた、複数個のマーカ候補の各頂点ｍ（ｉ）（ただし、ｍ（ｉ）＝１，・・・，Ｍ（ｉ））の画像座標系における座標（_１ｕ_ｉ，_１ｖ_ｉ）〜（_Ｍ（ｉ）ｕ_ｉ，_Ｍ（ｉ）ｖ_ｉ）、および世界座標系における既知の座標（_１ｘ_ｉ，_１ｙ_ｉ，_１ｚ_ｉ）〜（_Ｍ（ｉ）ｘ_ｉ，_Ｍ（ｉ）ｙ_ｉ，_Ｍ（ｉ）ｚ_ｉ）が入力される。位置推定部３１４は、これらを用い、第１実施形態で説明したように撮影装置１３の位置（基準点）の位置推定値ｃ＝（Ｘ_ｃ，Ｙ_ｃ，Ｚ_ｃ）を得る。位置推定部３１４は、さらに位置推定値ｃの精度を表す推定精度値ＡＣＣを得る。例えば、位置推定部３１４は、式（７）の［Ｒ｜ｔ］をＡとおき、その擬似逆行列をＡ^＋とし、式（９）のＥの二乗ノルムを推定精度値ＡＣＣとする。ただし、位置推定部３１４の処理の過程において、すべての頂点のカメラ座標系における座標（_１Ｌｘ_ｉ，_１Ｌｙ_ｉ，_１Ｌｚ_ｉ）〜（_Ｍ（ｉ）Ｌｘ_ｉ，_Ｍ（ｉ）Ｌｙ_ｉ，_Ｍ（ｉ）Ｌｚ_ｉ）を（Ｌｘ_１，Ｌｙ_１，Ｌｚ_１），・・・，（Ｌｘ_ｎ，Ｌｙ_ｎ，Ｌｚ_ｎ）とし、位置推定部３１４に入力されたすべての頂点の世界座標系における座標（_１ｘ_ｉ，_１ｙ_ｉ，_１ｚ_ｉ）〜（_Ｍ（ｉ）ｘ_ｉ，_Ｍ（ｉ）ｙ_ｉ，_Ｍ（ｉ）ｚ_ｉ）を（ｘ_１，ｙ_１，ｚ_１），・・・，（ｘ_ｎ，ｙ_ｎ，ｚ_ｎ）とする。推定精度値ＡＣＣが小さいほど推定精度が高い（ステップＳ３１１）。

《方位推定処理》
第１実施形態と同じである。

《制御情報の生成》
方位推定値Θｚおよび推定精度値ＡＣＣは制御部３１６に入力される。制御部３１６は、方位推定値Θｚが表す目的位置の方向成分を持つ向きに、加速度発生装置１２が振動による方向提示を行うための制御情報を生成して出力する。ただし、本実施形態の制御部３１６は、推定精度値ＡＣＣが示す推定精度が高いほど（推定精度値ＡＣＣが小さいほど）明確な方向提示または強い方向提示を行うような制御情報を生成して出力する。すなわち、推定精度値ＡＣＣが第１値のときの制御情報を第１の方向提示を行うための情報とし、推定精度値ＡＣＣが第２値のときの制御情報を第２の方向提示を行うための情報とし、第２値が表す精度が第１値が表す精度よりも高いとする。このとき、第２の方向提示を第１の方向提示よりも明確または強くする。

方向提示の明確性や強さを変化させる方法としては、例えば、加速度発生装置１２に供給する電流値や電圧値の振幅を制御する方法がある。すなわち、加速度発生装置１２に明確性が高い方向提示や強い方向提示を行わせるためには、振幅の大きな電流値や電圧値を加速度発生装置１２に供給し、逆に明確性が低い方向提示や弱い方向提示を行わせるためには、振幅の小さな電流値や電圧値を加速度発生装置１２に供給する。他の方法としては、加速度発生装置１２の振動子の偏加速度運動の周波数を制御する方法がある。８０Ｈｚまたは８０Ｈｚ付近の周波数成分は、動物（人を含む）の皮膚・筋・腱の受容器の中で、方向や加速度の知覚に寄与する受容器の神経活動を最も活発化させる周波数である。そのため、８０Ｈｚまたはその付近の周波数成分を持つ加速度が与えられた人（動物）は強い力覚や動きを知覚する。これを利用し、加速度発生装置１２に明確性が高い方向提示を行わせる際には、加速度発生装置１２の振動子が８０Ｈｚまたはその付近の周波数成分を持つ偏加速度運動を行う制御情報を生成し、逆に加速度発生装置１２に明確性が低い方向提示を行わせる際には、加速度発生装置１２の振動子が８０Ｈｚまたはその付近の周波数成分を持たない偏加速度運動を行う制御情報を生成してもよい。他の方法として、加速度発生装置１２に明確性が高い方向提示を行わせる際には、所望の方向の加速度の時間変化とその逆方向の加速度の時間変化とが非対称な偏加速度運動を行う制御情報を生成し、逆に加速度発生装置１２に明確性が低い方向提示を行わせる際には、所望の方向の加速度の時間変化とその逆方向の加速度の時間変化とが対称に近い加速度運動を行う制御情報を生成してもよい。また、これらの制御を組み合わせることで、方向提示の明確性と強さとを独立に制御してもよい。

なお、第３実施形態では第１実施形態の変形例として説明を行ったが、第３実施形態の構成を第２実施形態に適用してもよい。すなわち、第２実施形態の位置推定部２１４がさらに推定精度値ＡＣＣを生成して出力し、制御部１１６に代えて制御部３１６が推定精度値ＡＣＣが示す推定精度が高いほど明確な方向提示を行うような制御情報を生成して出力してもよい。

［第４実施形態］
第４実施形態は第１実施形態の変形例である。本実施形態では、人間の目では区別できないが特定の撮影装置（赤外線カメラ）で撮影された画像では区別可能なマーカを用いる。すなわち、本実施形態の周辺画像は、人間の目では区別できないマーカの相違を区別して撮影できる撮影装置で得られた画像である。以下では、第１実施形態との相違点を中心に説明し、第１実施形態と共通する事項については同じ参照番号を用いて説明を省略する。

図１に例示するように、本形態の位置推定システム４は、位置推定装置１１と加速度発生装置１２と撮影装置４３と複数個のマーカ４４−１〜４４−Ｎとを有する。

＜マーカ４４−１〜４４−Ｎ＞
マーカ４４−ｉ（ただし、ｉ＝１，・・・，Ｎ）は、第１実施形態のマーカ１４−ｉ（図２Ａ〜図２Ｃ）の白の部分に背景が黒色の再帰性反射材を貼り付けたものである。再帰性反射材とは入射された方向と同じに光を反射する素材のことである。カメラ等の撮影装置４３には赤外線発光の光源（たとえば赤外発光ＬＥＤ）が取り付けられ、撮像対象へ赤外線を照射している。赤外線は人間の目には直接見えないので、人間の目にはマーカ４４−ｉが一様の黒色に見える（図９Ａ）。しかし、一般のビデオカメラの多くのように赤外波長まで撮像可能な撮影装置４３では再帰性反射材だけが白く光り、マーカの部分だけが白く光る（図９Ｂ）。これにより、撮影される周辺画像からマーカ４４−ｉを抽出することができる。その他の構成は第１実施形態と同じである。

＜事前設定＞
マーカ１４−ｉがマーカ４４−ｉに置換される以外、第１実施形態と同じである。

＜処理＞
図７を用いて本実施形態の処理を説明する。
《撮影》
撮影装置４３が周囲を撮影し、それによってカラーの周辺画像（図３Ａ）を得る（ステップＳ４０１）。周辺画像はマーカ位置取得部１１３に送られる。

他の処理は第１実施形態と同じである。なお、第２実施形態や第３実施形態やそれらの変形例に本実施形態を適用してもよい。すなわち、第２実施形態や第３実施形態のマーカ１４−ｉやマーカ２４−ｉをマーカ４４−ｉに置換し、撮影装置１３を撮影装置４３に置換してもよい。

［第５実施形態］
カメラ等の撮影装置１３と加速度発生装置１２とが一体となっている場合（図６）、加速度発生装置１２によって生じる振動が安定した画像の取得に影響することがある。そのため、ビデオカメラに用いられているような手ぶれ補正のような画像の安定化が必要となる。本実施形態では、撮影位置の振動を表す情報を用いて周辺画像の振動成分を補正し、それによって得られる補正画像に由来するマーカの画像情報を用いる。以下では、第１実施形態にこのような変形を施した例を示すが、第２から４実施形態やそれらの変形例にこのような変形を施してもよい。

図１に例示するように、本実施形態の位置推定システム５は、第１実施形態の位置推定システム１の位置推定装置１１を位置推定装置５１に置換し、加速度センサ５６を加えたものである。位置推定装置５１は、第１実施形態の位置推定装置１１に振動検出部５１７および画像安定化部５１８を加えたものである。撮影装置１３と加速度発生装置１２とは一体化されている（図６）。

加速度センサ５６は、加速度発生装置１２の振動の大きさと方向を２軸で検出するセンサである。加速度センサ５６は、加速度発生装置１２に表面に配置され、その振動の方向と大きさを検出する。加速度センサ５６での検出信号は振動検出部５１７に送られ、加速度発生装置１２の振動の方向と大きさを表す情報（撮影位置の振動を表す情報）が画像安定化部５１８に送られる。画像安定化部５１８は、振動方向の方向に対して、ブラー・カーネルを算出し、カーネルの逆処理によって画像のぶれを除去するための情報をマーカ位置取得部１１３に送る。マーカ位置取得部１１３は、この情報を用いて撮影装置１３から送られた周辺画像を補正した補正画像に由来するマーカの画像情報を得る。これにより、画像の安定化を図る。なお、シャッタースピードが非常に高速なカメラを撮影装置１３として採用すれば安定した画像が取得できるため、そのような場合には本実施形態の構成および処理は不要である。

［特徴］
各実施形態の方式では、ＧＰＳ信号の使用が難しい室内であっても、簡易なマーカを天井や壁等に設置するだけで位置検出を行うことができる。さらに、マーカを天井や壁等に設置するだけで計測範囲を容易に広げることが可能であるため、コストに対する拡張性が高い。また、前述のように複数のマーカを同時に撮影して位置検出を行うため、推定精度が向上し、ロバスト性が上がる。加えて、振動による方向提示を行うことで、画面上の地図や指示などを注視する必要がないため、利用者の注意の遷移が小さい安全なナビゲーションを実現できる。

［その他の変形例等］
なお、本発明は上述の実施の形態に限定されるものではない。例えば、マーカが識別子を表すのではなく、マーカの世界座標系における座標（頂点の座標）そのものを表してもよい。この場合には、マーカの画像から直接頂点の座標を抽出できる。上記の構成の具体的実装形態として、カメラと振動提示装置と計算装置とを備えた携帯電話やスマートフォン等の携帯端末装置で上記の撮影装置と加速度発生装置と位置検出装置を構成してもよい。この場合、例えば、利用者がこのような携帯端末装置をそのカメラがマーカが設置された天井や壁を向くように手のひらに乗せて利用できる。また、撮影装置と加速度発生装置とが分離し、撮影装置に対する加速度発生装置の相対位置が固定されていなくてもよい。ただし、この場合には、撮影装置に対する加速度発生装置の相対位置に応じて式（８）のψｚを調整する必要がある。

上述の各種の処理は、記載に従って時系列に実行されるのみならず、処理を実行する装置の処理能力あるいは必要に応じて並列的にあるいは個別に実行されてもよい。その他、本発明の趣旨を逸脱しない範囲で適宜変更が可能であることはいうまでもない。

上述の位置推定装置をコンピュータによって実現する場合、位置推定装置が有すべき機能の処理内容はプログラムによって記述される。このプログラムをコンピュータで実行することにより、上記処理機能がコンピュータ上で実現される。この処理内容を記述したプログラムは、コンピュータで読み取り可能な記録媒体に記録しておくことができる。コンピュータで読み取り可能な記録媒体の例は、非一時的な（non-transitory）記録媒体である。このような記録媒体の例は、磁気記録装置、光ディスク、光磁気記録媒体、半導体メモリ等である。

このプログラムの流通は、例えば、そのプログラムを記録したＤＶＤ、ＣＤ−ＲＯＭ等の可搬型記録媒体を販売、譲渡、貸与等することによって行う。さらに、このプログラムをサーバコンピュータの記憶装置に格納しておき、ネットワークを介して、サーバコンピュータから他のコンピュータにそのプログラムを転送することにより、このプログラムを流通させる構成としてもよい。

このようなプログラムを実行するコンピュータは、例えば、まず、可搬型記録媒体に記録されたプログラムもしくはサーバコンピュータから転送されたプログラムを、一旦、自己の記憶装置に格納する。処理の実行時、このコンピュータは、自己の記録装置に格納されたプログラムを読み取り、読み取ったプログラムに従った処理を実行する。このプログラムの別の実行形態として、コンピュータが可搬型記録媒体から直接プログラムを読み取り、そのプログラムに従った処理を実行することとしてもよく、さらに、このコンピュータにサーバコンピュータからプログラムが転送されるたびに、逐次、受け取ったプログラムに従った処理を実行することとしてもよい。サーバコンピュータから、このコンピュータへのプログラムの転送は行わず、その実行指示と結果取得のみによって処理機能を実現する、いわゆるＡＳＰ（Application Service Provider）型のサービスによって、上述の処理を実行する構成としてもよい。

１〜５ 位置推定システム
１１〜３１,５１ 位置推定装置
１４−ｉ,２４−ｉ,４４−ｉ マーカ
１２ 加速度発生装置

Claims (10)

1. 基準点に対する相対位置が定められた撮影位置から撮影された周辺画像に由来する複数個のマーカの画像情報に基づいて複数個のマーカの位置情報を得るマーカ位置取得部と、
   前記複数個のマーカの位置情報に基づいて前記基準点の位置推定値を得る位置推定部と、
   を有する位置推定装置。
2. 請求項１の位置推定装置であって、
   前記マーカの画像情報はマーカの世界座標系における座標を特定するための情報を表し、
   前記マーカの位置情報は、前記マーカの世界座標系における座標を特定するための情報から特定される前記マーカの世界座標系における座標と、前記マーカの画像情報から特定される前記マーカの画像座標系における座標とを含む、位置推定装置。
3. 請求項１または２の位置推定装置であって、
   前記周辺画像は、複数の平面のそれぞれに位置するマーカの画像を含み、
   前記位置推定部は、同一の平面に位置する前記複数個のマーカの画像に由来する前記複数個のマーカの画像情報に基づいて前記基準点の位置の予備推定値を得、前記平面のそれぞれに対して得られた前記予備推定値を用いて前記位置推定値を得る、位置推定装置。
4. 請求項１から３の何れかの位置推定装置であって、
   前記周辺画像は、人間の目では区別できないマーカの相違を区別して撮影できる撮影装置で得られた画像である、位置推定装置。
5. 請求項１から４の何れかの位置推定装置であって、
   目的位置の情報と前記位置推定値とを用い、前記基準点または前記基準点に対する所定の相対位置を基点とした前記目的位置の方向を推定する方位推定部を有する、位置推定装置。
6. 請求項５の位置推定装置であって、
   前記目的位置の方向成分を持つ向きに振動による方向提示を行うための制御情報を得る制御部を有する、位置推定装置。
7. 請求項６の位置推定装置であって、
   前記位置推定部は、さらに前記位置推定値の精度を表す推定精度値を得、
   前記推定精度値が第１値のときの前記制御情報は、第１の方向提示を行うための情報であり、
   前記推定精度値が第２値のときの前記制御情報は、第２の方向提示を行うための情報であり、
   前記第２値が表す精度は、前記第１値が表す精度よりも高く、
   前記第２の方向提示は、前記第１の方向提示よりも明確または強い、位置推定装置。
8. 請求項１から７の何れかの位置推定装置であって、
   前記マーカの画像情報は、前記撮影位置の振動を表す情報を用いて前記周辺画像の振動成分を補正して得られる補正画像に由来する情報である、位置推定装置。
9. マーカ位置取得部で、基準点に対する相対位置が定められた撮影位置から撮影された周辺画像に由来する複数個のマーカの画像情報に基づいて複数個のマーカの位置情報を得るマーカ位置取得ステップと、
   位置推定部で、前記複数個のマーカの位置情報に基づいて前記基準点の位置推定値を得る位置推定ステップと、
   を有する位置推定方法。
10. 請求項１から８の何れかの位置推定装置としてコンピュータを機能させるためのプログラム。