JP6647481B2

JP6647481B2 - 位置姿勢推定装置及び位置姿勢推定方法

Info

Publication number: JP6647481B2
Application number: JP2019562441A
Authority: JP
Inventors: 隆博加島
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2018-02-13
Filing date: 2018-02-13
Publication date: 2020-02-14
Anticipated expiration: 2038-02-13
Also published as: JPWO2019159230A1; WO2019159230A1

Description

この発明は、主として歩行者が携帯するモバイル端末の位置姿勢を推定する位置姿勢推定装置及び位置姿勢推定方法に関する。

モバイル端末に内蔵された加速度センサと角速度センサを用いて、端末の相対的な位置の変位を測位する歩行者自律航法技術が存在する（例えば、非特許文献１参照）。また、このような歩行者自律航法は歩行量に比例して測位誤差が累積するため、地図情報を用いて測位結果を補正するマップマッチング技術を用いて、測位結果を最適と思われる位置に補正するようにしていた。

Daisuke KAMISAKA, Shigeki MURAMATSU, Takeshi IWAMOTO, Hiroyuki YOKOYAMA, Design and Implementation of Pedestrian Dead Reckoning System on a Mobile Phone, IEICE Transactions on Information and Systems, Released June 01, 2011, Online ISSN 1745-1361, Print ISSN 0916-8532

しかしながら、従来のマップマッチング技術では、壁や建物等の通行不可能な領域を通行するというような、現実では起こり得ない経路の測位を防ぐことはできるものの、通行可能な道路内におけるモバイル端末の正確な位置までは算出できないという課題があった。従って、例えば測位結果の位置情報を元に３次元グラフィックスを描画するような拡張現実（ＡＲ：ＡｕｇｍｅｎｔｅｄＲｅａｌｉｔｙ）アプリケーションでは、従来のマップマッチング技術を適用しても、道路内における端末位置の誤差により、正確に描画できないという課題があった。

この発明は、かかる問題を解決するためになされたもので、道路内における端末位置をより正確に推定することのできる位置姿勢推定装置を提供することを目的とする。

この発明に係る位置姿勢推定装置は、現在位置と現在姿勢の値を保持する位置姿勢記憶部と、地図上の道路形状を元に距離の基準となる基準路端と保持されている現在位置との距離を路端距離として求める路端距離算出部と、保持されている現在姿勢と基準路端の路端形状を元に進行方向を判定する進行方向判定部と、設定時間経過後の移動量を算出する移動量算出部と、保持されている現在位置と、路端距離算出部が算出した路端距離と基準路端と、進行方向判定部によって判定された進行方向と、移動量算出部で算出された移動量とを元に、保持されている現在位置から進行方向に基準路端の路端形状に沿って移動量分移動し、基準路端から路端距離離れた位置を算出し、算出した位置で位置姿勢記憶部の現在位置を更新する位置更新部とを備えたものである。

この発明に係る位置姿勢推定装置は、路端距離と基準路端と進行方向と移動量とを元に、現在位置から進行方向に基準路端の路端形状に沿って移動量分移動し、基準路端から路端距離離れた位置で現在位置を更新するようにしたので、道路内における端末位置を正確に推定することができる。

この発明の実施の形態１の位置姿勢推定装置のハードウェア構成図である。この発明の実施の形態１の位置姿勢推定装置の構成図である。この発明の実施の形態１の位置姿勢推定装置における絶対位置姿勢推定スレッドの流れを示すフローチャートである。この発明の実施の形態１の位置姿勢推定装置における進行方向判定スレッドの流れを示すフローチャートである。この発明の実施の形態１の位置姿勢推定装置における姿勢更新スレッドの流れを示すフローチャートである。この発明の実施の形態１の位置姿勢推定装置における位置更新スレッドの流れを示すフローチャートである。この発明の実施の形態１の位置姿勢推定装置における地図データベース上の道路形状を２次元平面にプロットした説明図である。この発明の実施の形態１の位置姿勢推定装置における道路方向の算出方法を示す説明図である。この発明の実施の形態１の位置姿勢推定装置における位置の更新方法を示す説明図である。この発明の実施の形態１の位置姿勢推定装置における基準路端の形状に沿った距離で更新位置を算出する方法を示す説明図である。この発明の実施の形態１の位置姿勢推定装置における直線距離で更新位置を算出する方法を示す説明図である。この発明の実施の形態２の位置姿勢推定装置の構成図である。

以下、この発明をより詳細に説明するために、この発明を実施するための形態について、添付の図面に従って説明する。
実施の形態１．
図１は、実施の形態１による位置姿勢推定装置を示すハードウェア構成図である。
図示の位置姿勢推定装置１００は、演算装置１０１、記憶装置１０２及びセンサ装置１０３から構成される。各装置はバス１０４により接続されている。
演算装置１０１は、例えばＣＰＵ（ＣｅｎｔｒａｌＰｒｏｃｅｓｓｉｎｇＵｎｉｔ）等のプロセッサである。記憶装置１０２は、例えば、ＲＡＭ（ＲａｎｄｏｍＡｃｃｅｓｓＭｅｍｏｒｙ）、ＲＯＭ（ＲｅａｄＯｎｌｙＭｅｍｏｒｙ）、フラッシュメモリ、ＨＤＤ（ＨａｒｄＤｉｓｋＤｒｉｖｅ）、ＳＳＤ（ＳｏｌｉｄＳｔａｔｅＤｒｉｖｅ）等であり、これらを組み合わせても良い。また、記憶装置の一部または全部を外部に設置し、インターネットやイントラネット等を経由して通信することで実現しても良い。センサ装置１０３は、３軸の加速度センサ及び３軸の角速度センサを用いて構成されている。角速度センサはジャイロスコープまたはジャイロセンサとも呼ばれる。
なお、図１の構成は最低限の構成であって、後述の絶対位置姿勢推定部の機能を実現するために、例えば撮像装置等を追加しても良い。

図２は、実施の形態１による位置姿勢推定装置の構成図である。
図示のように、位置姿勢推定装置は、センサ装置１０３、地図データベース１、絶対位置姿勢推定部２、位置姿勢記憶部３、進行方向判定部４、路端距離算出部５、路端距離記憶部６、移動量算出部７、位置更新部８、姿勢更新部９を備える。
センサ装置１０３は、図１のハードウェア構成図におけるセンサ装置１０３である。また、図２における絶対位置姿勢推定部２、進行方向判定部４、路端距離算出部５、移動量算出部７、位置更新部８、姿勢更新部９は、図１における記憶装置１０２に、それぞれの機能部に対応したプログラムが格納され、これらのプログラムを演算装置１０１で実行することにより実現される。あるいは、これら機能部のいずれかをハードウェアで構成しても良い。さらに、地図データベース１、位置姿勢記憶部３、路端距離記憶部６は、記憶装置１０２に設けられている。但しこれらの構成はあくまで一例であって、例えば地図データベース１を外部に設け、インターネットやイントラネット等を経由して位置姿勢推定装置に転送しても良い。

なお、以下の説明では位置姿勢推定装置を歩行者が持つモバイル端末の位置姿勢推定に用いるとして説明を行うが、歩行者以外の様々な移動体に適用しても良い。
また、以下の説明における「位置」とは２次元空間もしくは３次元空間における位置を示し、「姿勢」とは２次元空間もしくは３次元空間における姿勢（傾き、回転とも言う）を示す。２次元空間の場合は水平面の位置や姿勢のみを表す。３次元空間の場合は水平面に加え、垂直（高さ）の情報も表す。また、説明文中における「世界座標系」とは、原点（基準座標）が既知の座標系である。例えば国土地理院が定める平面直角座標系が該当するが、原点を独自に定めても良い。世界座標系の単位は例えばメートルやミリメートルが考えられるが、これらに限定するものではない。また、「路端」とは道路の端を意味し、トンネルであれば壁面を意味する。

地図データベース１は、世界座標系における道路の形状情報が記録されたデータベースである。道路の形状情報とは、道路の路端の形状を表したものである。
絶対位置姿勢推定部２は、位置姿勢推定装置の世界座標系における絶対的な位置と姿勢を推定するよう構成されている。位置と姿勢の推定手段としては、既存の手法を利用できる。例えば、設置位置が既知である専用のマーカもしくはその他画像認識可能な物体を図示しない撮像装置により撮影し、画像処理によりマーカもしくは物体を認識し、位置姿勢推定装置の位置姿勢を推定しても良い。または、ＧＰＳ（ＧｌｏｂａｌＰｏｓｉｔｉｏｎｉｎｇＳｙｓｔｅｍ）、ＢＬＥ（Ｂｌｕｅｔｏｏｔｈ（登録商標）ＬｏｗＥｎｅｒｇｙ）、ＵＷＢ（ＵｌｔｒａＷｉｄｅＢａｎｄ）、無線ＬＡＮ（Ｗｉ−Ｆｉとも言う）等の電波を使用する測位装置により位置姿勢推定装置の位置を推定し、加速度センサと地磁気センサにより位置姿勢推定装置の姿勢を推定する方法を用いても良い。

位置姿勢記憶部３は、絶対位置姿勢推定部２、位置更新部８及び姿勢更新部９によって算出された位置姿勢の値を記憶するための記憶部である。すなわち、位置姿勢記憶部３は、位置姿勢推定装置における現在の位置及び姿勢（以下、現在位置及び現在姿勢という）を保持する記憶部である。
進行方向判定部４は、位置姿勢記憶部３に記憶されている位置姿勢推定装置の位置姿勢を元に、位置姿勢推定装置の道路における進行方向を判定し、判定結果を位置更新部８に通知するよう構成されている。
路端距離算出部５は、絶対位置姿勢推定部２によって推定された位置姿勢推定装置の現在位置を元に、位置姿勢推定装置の路端からの距離（以下、路端距離という）と、距離の基準となる路端（以下、基準路端という）の位置を算出するよう構成されている。また、路端距離記憶部６は、路端距離算出部５によって算出された路端距離と基準路端の値を記憶する記憶部である。
移動量算出部７は、センサ装置１０３の加速度センサの値を元に、設定時間経過後の現在位置の値を算出するもので、その移動量として、位置姿勢推定装置を所持する利用者の歩行量を算出するよう構成されている。算出方法としては歩行者自律航法技術による既存の手段が利用可能である。移動量算出部７は１歩単位で歩行量を出力しても良いし、１歩分の歩行量を時間で分割して出力しても良い。
位置更新部８は、位置姿勢記憶部３が記憶する位置姿勢推定装置の現在の位置姿勢と、路端距離記憶部６が記憶する路端距離と、進行方向判定部４による進行方向と、移動量算出部７が算出した歩行量を元に、位置姿勢推定装置の位置を更新し、位置姿勢記憶部３に記憶された現在位置を更新するよう構成されている。
姿勢更新部９は、センサ装置１０３の角速度センサの値と、位置姿勢記憶部３が記憶する位置姿勢推定装置の現在姿勢を元に、位置姿勢推定装置の現在姿勢の値を更新し、位置姿勢記憶部３に記憶された現在姿勢を更新するよう構成されている。

次に、実施の形態１の位置姿勢推定装置の動作について説明する。
位置姿勢推定装置は、図３の絶対位置姿勢推定スレッドと、図４の進行方向判定スレッドと、図５の姿勢更新スレッドと、図６の位置更新スレッドとによる並列した処理を行う。
先ず、図３に示す絶対位置姿勢推定部２と路端距離算出部５による絶対位置姿勢推定スレッドの処理について説明する。
絶対位置姿勢推定部２は、先ず、位置姿勢推定装置の世界座標系における絶対位置姿勢を推定する（ステップＳＴ３０１）。絶対位置姿勢の推定方法は、前述の通り、既存の方法が利用可能である。推定に成功した場合（ステップＳＴ３０２−Ｙｅｓ）は次のステップＳＴ３０３に移り、失敗した場合（ステップＳＴ３０２−Ｎｏ）は再度ステップＳＴ３０１を実行する。

次に、絶対位置姿勢推定部２は、位置姿勢記憶部３が記憶する位置姿勢推定装置の現在位置及び現在姿勢を、ステップＳＴ３０１で推定した位置姿勢に更新する（ステップＳＴ３０３）。なお、位置姿勢記憶部３に位置姿勢が記憶されていない場合は、絶対位置姿勢推定部２の推定結果を記憶する（位置姿勢記憶ステップ）。
次に、路端距離算出部５により、ステップＳＴ３０１で推定した位置姿勢を元に、基準路端を判定する（ステップＳＴ３０４）。基準路端の算出方法について図７を用いて説明する。図７は、地図データベース１上の道路形状を２次元平面にプロットした図である。道路７０１は地図データベース１上の道路であり、路端７０２と路端７０３は、道路７０１の路端である。一般的に道路の方向は「上り」もしくは「下り」と呼ばれるため、本説明においても道路の方向を「上り」もしくは「下り」と呼ぶ。また、路端７０２と路端７０３を区別するため、上り方向を向いて道路上に立った場合に左側となる路端を左側路端と呼び、右側となる路端を右側路端と呼ぶこととする。図７においては、路端７０２は左側路端であり、路端７０３は右側路端となる。現在位置７０４は、位置姿勢記憶部３が記憶する位置姿勢推定装置の現在の位置を示す。ステップＳＴ３０３において、路端距離算出部５は、現在位置７０４に最も近い路端は左側路端か、もしくは右側路端かを判定する。そのため、現在位置７０４に最も近い両路端の位置、路端位置７０５と路端位置７０６を算出し、現在位置７０４との距離を算出する（路端距離算出ステップ）。図７においては、右側の路端位置７０６よりも左側の路端位置７０５の方が現在位置７０４に近いため、基準路端を左側路端と判定する。次に、路端距離算出部５はステップＳＴ３０３で判定した基準路端と、現在位置７０４と基準路端との距離を路端距離記憶部６に記憶させる（ステップＳＴ３０５）。その後、ステップＳＴ３０１に移り、上記のステップＳＴ３０１〜ステップＳＴ３０５の処理を繰り返す。
以上が絶対位置姿勢推定スレッドの処理である。

次に、図４に示す進行方向判定部４による進行方向判定スレッドの処理について説明する。
進行方向判定部４は、先ず、位置姿勢記憶部３に記憶されている位置姿勢推定装置の現在の位置姿勢を取得する（ステップＳＴ４０１）。位置姿勢記憶部３にまだ位置姿勢が記憶されていないため、現在の位置姿勢の値を取得できなかった場合（ステップＳＴ４０２−Ｎｏ）、処理はステップＳＴ４０１に戻る。一方、ステップＳＴ４０２において、現在の位置姿勢を取得できた場合（ステップＳＴ４０２−Ｙｅｓ）、進行方向判定部４は、路端距離記憶部６が記憶する基準路端を取得する（ステップＳＴ４０３）。
路端距離記憶部６がまだ基準路端を記憶していないため、進行方向判定部４が現在の基準路端を取得できなかった場合（ステップＳＴ４０４−Ｎｏ）、処理はＳＴ４０１に戻る。一方、ステップＳＴ４０４において、現在の基準路端を取得できた場合（ステップＳＴ４０４−Ｙｅｓ）、進行方向判定部４は位置姿勢推定装置の現在位置における道路の方向（以下、道路方向と言う）を算出する（ステップＳＴ４０５）。

道路方向は、上りもしくは下りの方向の角度（方位）で表される。本説明では、上り方向の角度とする。道路方向の算出方法について図８を用いて説明する。図８は図７と同様に、地図データベース１上の道路形状を２次元平面にプロットした図である。本説明では、ステップＳＴ４０３で取得した基準路端を左側路端の路端７０２とする。ステップＳＴ４０５では、まず、現在位置から最も近い基準路端の位置を算出する。図８では路端位置７０５が現在位置から最も近い基準路端の位置であるとする。そして、路端位置７０５における上り方向への角度８０１を算出し、この角度を道路方向とする。なお、図８ではｘ軸方向を角度の基準としたが、実際には任意の角度を基準にできる。
次に、進行方向判定部４は、位置姿勢推定装置の現在姿勢を元に、位置姿勢推定装置の方向（以下、装置方向と言う）を算出する（ステップＳＴ４０６）。装置方向は装置によって定義が異なるが、例えば、利用者のモバイル端末がタブレット端末やスマートフォン端末であれば、これら端末の背面を装置方向と定義する。装置方向は、ステップＳＴ４０５の道路方向と同様に、角度（方位）で表す。

次に、進行方向判定部４は、位置姿勢推定装置が道路に対して上りもしくは下りのどちらを向いているのかを判定する（ステップＳＴ４０７：進行方向判定ステップ）。この判定方法としては、先ず、ステップＳＴ４０５で算出した道路方向と、ステップＳＴ４０６で算出した装置方向との差を算出する。なお、道路方向と装置方向との差は角度であるため、計算結果を−１８０°から１８０°に正規化する。次に、両方向の差が−９０°から９０°の範囲内であれば、位置姿勢推定装置は道路に対して上り方向を向いていると判定する。もしくは、両方向の差が−９０°から９０°の範囲外、すなわち、−１８０°から−９０°の範囲内、もしくは、９０°から１８０°の範囲内であれば、位置姿勢推定装置は道路に対して下り方向を向いていると判定する。進行方向判定部４の判定の結果は位置更新部８に通知する。ステップＳＴ４０７で進行方向を判定した後はステップＳＴ４０１に移り、上記のステップＳＴ４０１〜ステップＳＴ４０７の処理を繰り返す。
以上が、進行方向判定部４による進行方向判定スレッドの処理である。

次に、図５に示す姿勢更新部９による姿勢更新スレッドの処理について説明する。
姿勢更新部９は、先ず、位置姿勢記憶部３が記憶する位置姿勢推定装置の現在姿勢を取得する（ステップＳＴ５０１）。位置姿勢記憶部３にまだ姿勢が記憶されていないため、現在姿勢を取得できなかった場合（ステップＳＴ５０２−Ｎｏ）、処理はステップＳＴ５０１に戻る。一方、ステップＳＴ５０２において現在姿勢を取得できた場合（ステップＳＴ５０２−Ｙｅｓ）、姿勢更新部９は、センサ装置１０３の角速度センサから角速度を取得する（ステップＳＴ５０３）。次に、姿勢更新部９は、ステップＳＴ５０１で取得した現在姿勢に対して、ステップＳＴ５０３で取得した現在の角速度を元に、現在姿勢の値を更新し、結果を位置姿勢記憶部３に通知して更新する（ステップＳＴ５０４）。その後はステップＳＴ５０１に戻り、これらステップＳＴ５０１〜ステップＳＴ５０４の処理を繰り返す。
以上が、姿勢更新部９による姿勢更新スレッドの処理である。

次に、図６に示す位置更新部８による位置更新スレッドの処理について説明する。
位置更新部８は、先ず、位置姿勢記憶部３に記憶されている位置姿勢推定装置の現在位置を取得する（ステップＳＴ６０１）。位置姿勢記憶部３にまだ現在位置が記憶されていないため、現在位置を取得できなかった場合（ステップＳＴ６０２−Ｎｏ）、処理はＳＴ６０１に戻る。一方、ステップＳＴ６０２において、現在位置を取得できた場合（ステップＳＴ６０２−Ｙｅｓ）、移動量算出部７が、センサ装置１０３の加速度センサから加速度の値を取得する（ステップＳＴ６０３）。次に、移動量算出部７は、ステップＳＴ６０３で取得した加速度を元に、歩行量を算出する（ステップＳＴ６０４：移動量算出ステップ）。

歩行量の算出には、前述の通り、歩行者自律航法技術による既存の方法を利用できる。ステップＳＴ６０５で、移動量算出部７が利用者は歩行していないと判定した場合（ステップＳＴ６０５−Ｎｏ）、処理はステップＳＴ６０１に戻る。一方、ステップＳＴ６０５において、移動量算出部７が利用者は歩行していると判定した場合（ステップＳＴ６０５−Ｙｅｓ）、位置更新部８は、路端距離記憶部６が記憶する基準路端と路端距離を取得する（ステップＳＴ６０６）。路端距離記憶部６にまだ基準路端と路端距離が記憶されていないため、基準路端と路端距離を取得できなかった場合（ステップＳＴ６０７−Ｎｏ）、処理はステップＳＴ６０１に戻る。一方、ステップＳＴ６０７において、位置更新部８が基準路端と路端距離を取得できた場合（ステップＳＴ６０７−Ｙｅｓ）、位置更新部８は進行方向判定部４が判定した位置姿勢推定装置の進行方向を取得する（ステップＳＴ６０８）。進行方向判定部４がまだ進行方向を判定していないため、進行方向を取得できなかった場合（ステップＳＴ６０９−Ｎｏ）、処理はＳＴ６０１に戻る。一方、ステップＳＴ６０９において進行方向を取得できた場合（ステップＳＴ６０９−Ｙｅｓ）、位置更新部８は、ステップＳＴ６０４で移動量算出部７が算出した歩行量と、ステップＳＴ６０６で取得した基準路端と路端距離と、ステップＳＴ６０８で取得した進行方向とを元に、位置姿勢推定装置の位置を更新する（ステップＳＴ６１０：位置更新ステップ）。

ステップＳＴ６１０における位置の更新方法を図９を用いて説明する。図９は、地図データベース１上の道路形状を２次元平面にプロットした図である。道路９０１は地図データベース１上の道路であり、路端９０２と路端９０３は、道路９０１の路端である。図９の例においては、基準路端は左側路端の路端９０２とする。位置９０４は、位置姿勢記憶部３が記憶する位置姿勢推定装置の現在位置である。路端距離９０５は、路端距離記憶部６が記憶する路端距離である。また、図９の例においては、進行方向は上り方向であるものとする。この状況において、位置更新部８は、現在位置９０４から上り方向に歩行量９０６の距離を移動させ、基準路端９０２から路端距離９０５である更新位置９０７を算出する。そして、更新位置９０７を位置姿勢記憶部３に通知し、位置姿勢推定装置の位置を更新する。

なお、ステップＳＴ６１０において、現在位置９０４と更新位置９０７との歩行量９０６の扱いについては、基準路端９０２の形状に沿った距離、もしくは、単純な直線距離のどちらかが考えられるが、どちらを採用しても良い。この違いを図１０と図１１を使用して説明する。図１０と図１１は図９と同様に、地図データベース１上の道路形状を２次元平面にプロットした図であるが、図１０と図１１は図９とは異なる形状の道路で例示している。図１０は基準路端９０２の形状に沿った距離で更新位置を算出する方法を示す。基準路端９０２の形状に沿った距離で位置を算出する場合、まず基準路端９０２から路端距離９０５の距離分、道路の中央側に移動した形状１００１を算出する。そして、現在位置９０４から形状１００１上で歩行量１００２分移動した更新位置１００３を算出する。一方、図１１は単純に直線距離で更新位置を算出する方法を示す。直線距離で更新位置を算出する場合、現在位置９０４から直線距離で歩行量１１０１分移動した路端距離９０５である更新位置１１０２を算出する。路端形状が直線であればどちらの算出方法でも差異は生じないが、路端距離が曲線の場合は距離の算出方法によって更新位置に違いが生じる。位置更新スレッドによる更新頻度が、１歩分の歩行か、もしくは１歩分を分割した単位で更新する場合、大きな違いは生じないため、どちらの方法でも実用的である。一方で位置更新スレッドによる更新頻度が少ない場合、例えば２歩以上の歩行単位で更新する場合は、基準路端９０２の形状に沿った距離で位置を算出する方が、実際の歩行距離に近いため好ましい。
位置更新部８は、ステップＳＴ６１０で現在位置を更新した後、ステップＳＴ６０１に移り、上記のステップＳＴ６０１〜ステップＳＴ６１０の処理を繰り返す。
以上が、位置更新部８による位置更新スレッドの処理である。

このように構成された位置姿勢推定装置により、始めに絶対位置姿勢推定スレッドによって絶対位置姿勢が推定され、位置姿勢記憶部３に記憶されると同時に、基準となる路端と路端距離が路端距離記憶部６に記憶される。その後、進行方向判定スレッドと姿勢更新スレッドによって、進行方向と姿勢が常時更新される。また、位置更新スレッドによって、絶対位置姿勢を推定した際に記憶された基準路端と路端距離を元に、現在位置から基準路端の路端形状に沿って進行方向に向かって歩行量分移動し、基準路端から路端距離分離れた位置を算出し、位置姿勢記憶部３が記憶する位置を更新する。従って、絶対位置姿勢推定部２による絶対位置姿勢が不可能な状況になった場合でも、位置と姿勢を現在の値に更新できる。また、このように構成された位置姿勢推定装置は、基準路端の路端形状に沿って、かつ、基準路端から路端距離分離れた位置を算出することから、既存のマップマッチングを適用した歩行者自律航法技術に比べて、道路上の実際の歩行位置を正確に推定できる。この位置姿勢推定装置で正確な位置姿勢を推定するには、位置姿勢推定装置を所持する利用者が、路端からの距離を一定に保ちながら、路端に沿って、上りまたは下り方向に歩行をするという制約を満たす必要があるが、一般的に自動車道の路側帯を歩行者が歩行する場合、自ずとこのような制約下で歩行しなければならないため、道路内における端末位置を正確に推定することができるという効果は失われることがない。
また、進行方向の判定には角速度センサの値を使用するが、上りもしくは下りどちらかの判定であるため、角速度に多少の誤差が付加されていても許容できる。これにより、例えば拡張現実のように、モバイル端末の正確度の高い位置姿勢が求められるアプリケーションでも適用できる。

以上説明したように、実施の形態１の位置姿勢推定装置によれば、現在位置と現在姿勢の値を保持する位置姿勢記憶部と、地図上の道路形状を元に距離の基準となる基準路端と保持されている現在位置との距離を路端距離として求める路端距離算出部と、保持されている現在姿勢と基準路端の路端形状を元に進行方向を判定する進行方向判定部と、設定時間経過後の移動量を算出する移動量算出部と、保持されている現在位置と、路端距離算出部が算出した路端距離と基準路端と、進行方向判定部によって判定された進行方向と、移動量算出部で算出された移動量とを元に、保持されている現在位置から進行方向に基準路端の路端形状に沿って移動量分移動し、基準路端から路端距離離れた位置を算出し、算出した位置で位置姿勢記憶部の現在位置を更新する位置更新部とを備えたので、道路内における端末位置を正確に推定することができる。

また、実施の形態１の位置姿勢推定装置によれば、現在位置及び現在姿勢として絶対位置姿勢を推定する絶対位置姿勢推定部を備えたので、より正確に道路内における端末位置を推定することができる。

また、実施の形態１の位置姿勢推定装置によれば、位置姿勢記憶部が記憶する現在姿勢と角速度センサによって得られる角速度値とを元に新たな現在姿勢を算出し、新たな現在姿勢で位置姿勢記憶部の現在姿勢を更新する姿勢更新部を備えたので、現在姿勢も更新されるため、より正確に端末の進行方向も推定することができる。

また、実施の形態１の位置姿勢推定方法によれば、位置姿勢記憶部が現在位置と現在姿勢の値を保持する位置姿勢記憶ステップと、路端距離算出部が、地図上の道路形状を元に距離の基準となる基準路端と保持されている現在位置との距離を路端距離として求める路端距離算出ステップと、進行方向判定部が、保持されている現在姿勢と基準路端の路端形状を元に進行方向を判定する進行方向判定ステップと、移動量算出部が、設定時間経過後の移動量を算出する移動量算出ステップと、位置更新部が、保持されている現在位置と、路端距離算出ステップで算出した路端距離と基準路端と、進行方向判定ステップによって判定された進行方向と、移動量算出ステップで算出された移動量とを元に、保持されている現在位置から進行方向に基準路端の路端形状に沿って移動量分移動し、基準路端から路端距離離れた位置を算出し、算出した位置で位置姿勢記憶ステップの現在位置を更新する位置更新ステップとを備えたので、道路内における端末位置を正確に推定することができる。

実施の形態２．
実施の形態２は、現在位置の高度も含めて推定するようにしたものである。
図１２は、実施の形態２の位置姿勢推定装置の構成図である。
図示の位置姿勢推定装置は、地図データベース１、絶対位置姿勢推定部２ａ、位置姿勢記憶部３ａ、進行方向判定部４、路端距離算出部５、路端距離記憶部６、移動量算出部７、位置更新部８ａ、姿勢更新部９、地上高算出部１０、地上高記憶部１１を備える。
絶対位置姿勢推定部２ａは、現在位置の３次元位置を推定する機能を有している。位置姿勢記憶部３ａは、高度も含めて現在位置の３次元位置の値を記憶する。地上高算出部１０は、絶対位置姿勢推定部２ａで推定した絶対位置の高度と地図データベース１における地図上の標高を元に、現在位置の地上高を算出する。また、地上高記憶部１１は地上高算出部１０で算出した地上高を記憶する記憶部である。位置更新部８ａは、現在位置の更新を行う際、地図上の標高と、地上高記憶部１１に記憶されている現在位置の地上高を元に新たな高度を算出し、位置姿勢記憶部３ａに記憶されている高度を更新するよう構成されている。他の構成は図２に示した実施の形態１の構成と同様であるため、対応する部分に同一符号を付してその説明を省略する。

なお、実施の形態２において、「高度」とは、絶対的な高さを表す値を意味する。一般的には海抜を基準とした高さであるが、他の基準を使用しても良い。また、「標高」とは、地図データベース１上に格納されている地表面の高度を意味する。また、「地上高」とは、標高を基準とした高さを意味し、例えば地上高１メートルとは、地表面から１メートルの高さを示す。

次に、実施の形態２の位置姿勢推定装置の動作について説明する。
絶対位置姿勢推定部２ａは、絶対位置姿勢の推定に成功した際に、推定結果の高度を位置姿勢記憶部３ａと地上高算出部１０に出力する。地上高算出部１０では、絶対位置姿勢推定部２ａから取得した高度と地図データベース１上の標高を元に、位置姿勢推定装置の地上高を算出し、地上高記憶部１１に記憶させる。
位置更新部８ａは、現在位置を更新する際に、実施の形態１の処理による２次元平面の位置を元に、地図データベース１上の標高を求める。そして、位置更新部８ａは、標高に対して地上高記憶部１１が記憶する地上高を加算した高度を位置姿勢推定装置の高さとし、位置姿勢記憶部３ａに記憶させる。その他の動作は、実施の形態１と同様であるため、ここでの説明は省略する。

このように構成された位置姿勢推定装置により、絶対位置姿勢推定部２ａにより３次元の絶対位置姿勢が推定された際に、位置姿勢推定装置の地上高が記憶される。また、位置更新部８ａにより位置を更新する際に、地図データベース１上の標高に対して、絶対位置姿勢が推定された際に記憶した地上高が加算された高度に更新される。従って、位置姿勢推定装置を所持する利用者が、位置姿勢推定装置の高さを変えない限り、位置更新部８ａは実際の高度を算出する。また、位置更新部８ａは地図データベース１上の標高を元に高度を算出するため、道路が平坦ではなく凹凸や勾配があっても、正確な高度を算出できる。

以上説明したように、実施の形態２の位置姿勢推定装置によれば、絶対位置姿勢推定部は、高度を含む絶対位置を推定すると共に、絶対位置の高度と地図上の標高を元に、現在位置の地上高を算出する地上高算出部を備え、位置更新部は、現在位置を更新する際に、地図上の標高と、現在位置の地上高を元に新たな高度を算出するようにしたので、高度を含めて道路内における端末位置を正確に推定することができる。

なお、本願発明はその発明の範囲内において、各実施の形態の自由な組合せ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。

以上のように、この発明に係る位置姿勢推定装置及び位置姿勢推定方法は、歩行者が携帯するモバイル端末等の位置姿勢を推定する構成に関するものであり、例えば、拡張現実アプリケーションでの描画に用いるのに適している。

１地図データベース、２，２ａ絶対位置姿勢推定部、３，３ａ位置姿勢記憶部、４進行方向判定部、５路端距離算出部、６路端距離記憶部、７移動量算出部、８，８ａ位置更新部、９姿勢更新部、１０地上高算出部、１１地上高記憶部。

Claims

現在位置と現在姿勢の値を保持する位置姿勢記憶部と、
地図上の道路形状を元に距離の基準となる基準路端と前記保持されている現在位置との距離を路端距離として求める路端距離算出部と、
前記保持されている現在姿勢と前記基準路端の路端形状を元に進行方向を判定する進行方向判定部と、
設定時間経過後の移動量を算出する移動量算出部と、
前記保持されている現在位置と、前記路端距離算出部が算出した前記路端距離と前記基準路端と、前記進行方向判定部によって判定された進行方向と、前記移動量算出部で算出された移動量とを元に、前記保持されている現在位置から前記進行方向に前記基準路端の路端形状に沿って前記移動量分移動し、前記基準路端から前記路端距離離れた位置を算出し、当該算出した位置で前記位置姿勢記憶部の現在位置を更新する位置更新部とを備えたことを特徴とする位置姿勢推定装置。
前記現在位置及び前記現在姿勢として絶対位置姿勢を推定する絶対位置姿勢推定部を備えたことを特徴とする請求項１記載の位置姿勢推定装置。
前記位置姿勢記憶部が記憶する現在姿勢と角速度センサによって得られる角速度値とを元に新たな現在姿勢を算出し、当該新たな現在姿勢で前記位置姿勢記憶部の現在姿勢を更新する姿勢更新部を備えたことを特徴とする請求項１または請求項２記載の位置姿勢推定装置。
前記絶対位置姿勢推定部は、高度を含む絶対位置を推定すると共に、
前記絶対位置の高度と地図上の標高を元に、現在位置の地上高を算出する地上高算出部を備え、
前記位置更新部は、現在位置を更新する際に、地図上の標高と、前記現在位置の地上高を元に新たな高度を算出することを特徴とする請求項２記載の位置姿勢推定装置。
位置姿勢記憶部が現在位置と現在姿勢の値を保持する位置姿勢記憶ステップと、
路端距離算出部が、地図上の道路形状を元に距離の基準となる基準路端と前記保持されている現在位置との距離を路端距離として求める路端距離算出ステップと、
進行方向判定部が、前記保持されている現在姿勢と前記基準路端の路端形状を元に進行方向を判定する進行方向判定ステップと、
移動量算出部が、設定時間経過後の移動量を算出する移動量算出ステップと、
位置更新部が、前記保持されている現在位置と、前記路端距離算出ステップで算出した前記路端距離と前記基準路端と、前記進行方向判定ステップによって判定された進行方向と、前記移動量算出ステップで算出された移動量とを元に、前記保持されている現在位置から前記進行方向に前記基準路端の路端形状に沿って前記移動量分移動し、前記基準路端から前記路端距離離れた位置を算出し、当該算出した位置で前記位置姿勢記憶ステップの現在位置を更新する位置更新ステップとを備えたことを特徴とする位置姿勢推定方法。