進行方向推定装置及び進行方向推定方法

本発明は、歩行者等の移動体の進行方向を推定する技術に関する。

従来から、歩行者等の移動体が保持する端末に内蔵されている加速度センサ、磁気センサ、ジャイロ、気圧センサ等のセンシング装置による計測データに基づいて、世界座標系に対する移動体の移動方向を推定する技術が考案されている(非特許文献1)。

世界座標系に対する移動体の移動方向を推定するためには、センシング装置のセンサ座標系における、(1)重力方向ベクトルと、(2)真北方向等を指す水平基準方向ベクトル、(3)進行方向ベクトルの3つのベクトルを同時に推定する必要がある。上記(1)〜(3)のベクトルについて推定(トラッキング)することで、水平基準方向に対する進行方向のなす角の推定がなされ、移動体の移動方位角の推定を行うことが可能となる。

上記(1)の重力方向ベクトルは、加速度センサ、角速度センサからそれぞれ出力される3軸データに基づいて、センサ座標系における重力方向のトラッキングを行うことができる。また、上記(2)の水平基準方向ベクトルは、加速度センサ、角速度センサ、磁気センサに基づいて、水平基準方向のトラッキングを行うことが可能である。例えば、非特許文献1には、重力方位ベクトル及び水平基準方向ベクトルのトラッキングを行う手法が具体的に開示されている。また、いわゆるAHRS(Attitude and Heading Reference System)に基づく手法も知られている。

また、上記(3)の進行方向ベクトルにおいては、センシング装置を移動体である歩行者の足先や腰部に固定する場合と、歩行者がセンシング装置の内蔵された端末を保持する姿勢を自由に変えた場合とでは、考慮すべき技術的課題が大きく異なる。

例えば、センシング装置を固定している場合では、当該装置と歩行者との位置関係が固定されるため、移動体の進行方向のトラッキングは概ね既知となり、比較的容易である。しかし、センシング装置が保持される位置や保持される姿勢が自由に変わる場合においては、進行方向をトラッキングすることは容易ではない。

そこで、進行方向を推定するために、これまで様々な方法が提案されている(特許文献1及び特許文献2)が、これらの方法によっても精度よく進行方向を推定することはできなかった。

また、人の歩行動作における加速度成分及び角速度成分のパターンについては、加速度ベクトル(3軸)と角速度ベクトル(3軸)をその進行方向と鉛直方向(重力方向)、及びそれぞれに直交する左右方向(横方向)へ成分分解すると、それぞれの成分は以下の表1のような周波数特性をもつことが非特許文献2において開示されている。

つまり、加速度ベクトルと角速度ベクトルを、表1の性質を満たすように成分分解できるとき、その方向は、正確な進行方向と左右方向(横方向)であると考えることができる。

ここで、非特許文献2においては、進行方向の推定アルゴリズムとして、連続的に方位角を必要な分解能だけ変動させることを提案している。

特開2009−156660号公報

特開2011−237452号公報

「歩行者ナビのための自蔵センサモジュールを用いた屋内測位システムとその評価」シンポジウム「モバイル08」予稿集,pp.151-156 (2008)

「歩行者デッドレコニングに基づくハンドヘルド端末の屋内外測位技術」信学技報MVE2010-96,pp.171-176 (2011)

しかし、上記のように、進行方向の推定アルゴリズムとして重力に直交する方位角をパラメータとして変動させることにより真の進行方向を探索する方法を採用すると、計算コストが大きくなると共に進行方向の推定に時間がかかるという問題があった。

本発明は、このような問題を解決するためになされたもので、より効率的に移動体の進行方向を推定することのできる装置及び方法を提供することを目的とする。

上記課題を解決するために、本発明は、移動体が進行する向きを推定する進行方向推定装置であって、上記進行による移動体の加速度を検知する加速度検知手段と、上記進行による移動体の角速度を検知する角速度検知手段と、加速度検知手段により検知された加速度及び角速度検知手段により検知された角速度に基づいて、水平面上で上記進行する向きと直交する向き及び上記進行する向きにおける移動体の加速度成分の大きさと、直交する向き及び上記進行する向きにおける移動体の角速度成分の大きさの少なくとも一方に応じた目的関数を演算し、目的関数が最大となる方向を算出すると共に、移動体の加速度の鉛直方向成分と上記進行する向きの成分における位相差に基づいて、上記方向のうち移動体が進行する向きを決定する進行方向決定手段とを備えた進行方向推定装置を提供する。

また、上記課題を解決するために、本発明は、移動体が進行する向きを推定する進行方向推定方法であって、水平面上で上記進行する向きと直交する向き及び上記進行する向きにおける移動体の加速度成分の大きさと、上記直交する向き及び上記進行する向きにおける移動体の角速度成分の大きさの少なくとも一方に応じた目的関数を設定する第一のステップと、目的関数が最大値となる移動体が進行する方向を算出する第二のステップと、移動体の加速度の鉛直方向成分と上記進行する向きの成分における位相差に基づいて、第二のステップで算出された上記進行する方向のうち移動体が進行していると推定される上記向きを決定する第三のステップとを有する進行方向推定方法を提供する。

本発明によれば、従来より効率的に、すなわち処理コストが低く、かつ高速に移動体の進行方向を推定できる進行方向推定装置及び進行方向推定方法を提供することができる。

本発明の実施の形態に係る進行方向推定装置の構成を示すブロック図である。

図1に示された1次元FFT演算部9により得られた加速度成分のパワースペクトラムの一例を示す図である。

図1に示された進行方向推定装置の動作を示すフローチャートである。

図3に示されたステップS1における前処理の動作を示すフローチャートである。

図3に示されたステップS2における算出方法を示すフローチャートである。

水平基準方位ベクトルと進行方向ベクトルと重力方向ベクトルの位置関係を示す図である。

以下、本発明の実施の形態について、図面を参照して詳しく説明する。なお、図中同一符号は同一又は相等部分を示す。 図1は、本発明の実施の形態に係る移動体の進行方向推定装置の構成を示すブロック図である。

図1に示された進行方向推定装置は、計測手段として、歩行者等の移動体に装着又は保持されるスマートフォン又は携帯端末等の端末装置に取り付けられた加速度センサ(3軸)1と角速度センサ(3軸)3を備える。これらのセンサは必ずしも同一の座標系における出力が得られる必要はないが、最終的には座標変換を施して、同一の直交座標系(X/Y/Z軸)で扱えるように変換されているものとする。

加速度センサを構成する加速度成分(X)測定部11、加速度成分(Y)測定部12、加速度成分(Z)測定部13はそれぞれ、移動体に対してその相対姿勢角の関係が既知であるセンサ座標系のX軸、Y軸、Z軸における加速度のベクトル成分データを計測して出力する。同様に、角速度成分(X)測定部31、角速度成分(Y)測定部32、角速度成分(Z)測定部33はそれぞれ、X軸、Y軸、Z軸における角速度成分データを計測して出力する。加速度のベクトル成分データ及び角速度のベクトル成分データは、必ずしもセンサから出力された成分データそのものである必要はなく、たとえば、加速度ベクトルと角速度ベクトルのオフセットや感度の校正処理後のデータを出力してもよい。こうした処理として、別途内蔵された温度センサによって温度補償の校正処理を実現する構成を取ってもよい。

重力方向ベクトル推定部5は、加速度センサ(3軸)1と角速度センサ(3軸)3で計測されたデータから得られる加速度ベクトル及び角速度ベクトルに基づいて、本進行方向推定装置のセンサ座標系における重力方向ベクトルを推定して出力する。このような処理は、従来から行われている手法によって実現可能である。

(加速度成分)重力方向ベクトル射影計算部7は、加速度センサ(3軸)1から出力される3軸における各加速度成分と重力方向ベクトル推定部5が推定する重力方向ベクトルを入力し、加速度ベクトルのうち重力加速度ベクトル(=1G)を差し引いた後の加速度ベクトルのうち、重力方向ベクトルへ射影される成分の大きさを計算して出力する。

バッファ8は、(加速度成分)重力方向ベクトル射影計算部7から出力されたデータをバッファリングして格納する1次元バッファである。バッファリングは先入れ先出し(FIFO)によって実現されることが一般的ではあるが、格納された時間順が保持されるバッファであれば十分である。以下のバッファ15〜17,35〜37についても同様である。

バッファ15は、加速度成分(X)測定部11及び重力方向ベクトル推定部5から出力されたデータを格納する1次元バッファである。バッファ16は、加速度成分(Y)測定部12及び重力方向ベクトル推定部5から出力されたデータを格納する1次元バッファである。バッファ17は、加速度成分(Z)測定部13及び重力方向ベクトル推定部5から出力されたデータを格納する1次元バッファである。バッファ35は、角速度成分(X)測定部31から出力されたデータを格納する1次元バッファである。バッファ36は、角速度成分(Y)測定部32から出力されたデータを格納する1次元バッファである。バッファ37は、角速度成分(Z)測定部33から出力されたデータを格納する1次元バッファである。

1次元FFT演算部9は、バッファ8に格納された射影成分に基づいて、定められたサンプルデータ長(たとえば512サンプル)について1次元離散フーリエ変換演算を実行し、その周波数領域変換されたデータを出力する。つまり、1次元FFT演算部9は、1次元離散フーリエ変換演算を行うことで、バッファ8に格納された加速度成分のパワースペクトラムのピーク周波数を算出する。なお、本パワースペクトラムの一例が図2に示される。

ここで、図2に示されるパワースペクトラムにおいては、縦軸がパワー[m/s²]とされ、横軸が周波数[Hz]とされる。そして、後述するように、本パワースペクトラムに基づいて、歩行周波数・位相推定部10により2Hzが基本周波数と推定される。

一方、離散フーリエ変換によって周波数を正しく決定するためにはサンプリング周波数に関する情報が必要である。このデータに関してはバッファ8に格納される段階で保持されていてもよいし、事前に外部の記憶装置に格納された情報を読み出すことにより該データを取得してもよい。以下の1次元FFT演算部19〜21,39〜41についても、同様である。

1次元FFT演算部19は、バッファ15に格納されたデータに基づいて、定められたサンプルデータ長について1次元離散フーリエ変換演算を実行し、その周波数領域変換されたデータを出力する。同様に1次元FFT演算部20は、バッファ16に格納されたデータに基づいて、定められたサンプルデータ長について1次元離散フーリエ変換演算を実行し、その周波数領域変換されたデータを出力する。1次元FFT演算部21は、バッファ17に格納されたデータに基づいて、定められたサンプルデータ長について1次元離散フーリエ変換演算を実行し、その周波数領域変換されたデータを出力する。1次元FFT演算部39は、バッファ35に格納されたデータに基づいて、定められたサンプルデータ長について1次元離散フーリエ変換演算を実行し、その周波数領域変換されたデータを出力する。1次元FFT演算部40は、バッファ36に格納されたデータに基づいて、定められたサンプルデータ長について1次元離散フーリエ変換演算を実行し、その周波数領域変換されたデータを出力する。1次元FFT演算部41は、バッファ37に格納されたデータに基づいて、定められたサンプルデータ長について1次元離散フーリエ変換演算を実行し、その周波数領域変換されたデータを出力する。

歩行周波数・位相推定部10は、1次元FFT演算部9が算出する重力方向の加速度成分の離散フーリエ変換データに基づいて歩行動作の基本周波数(歩行周波数)を推定する。

ここで、歩行周波数は所定の数値範囲内に入ることが経験的に知られているため、当該範囲内でパワーが最大となる成分が持つ周波数を歩行周波数と推定することができる。

具体的には、一般的な人の歩行動作の周期(0.3秒〜1.6秒につき1歩程度、つまり0.7〜3.0Hz)の範囲内に特徴的な周波数ピークが存在する場合、その周波数を歩行周波数と推定できる。また、時間領域上で歩行動作を一歩単位で検出することで、その歩行周波数の概算を求め、周波数領域上での対応する周波数ピークを検出することにより、その時間間隔から歩行周波数を決定してもよい。

以上のように歩行周波数を推定した後は、その周波数ピークについて位相を求め、歩行周波数とともに出力する。

進行方向ベクトル決定部50は、歩行周波数・位相推定部10から出力された歩行周波数及び位相を示すデータと、1次元FFT演算部19〜21,39〜41から出力された各離散フーリエ変換データと、重力方向ベクトル推定部5から出力された重力方向ベクトルとを入力し、後述する方法によって、移動体の進行方向ベクトルを決定する。

以下において、図1に示された進行方向推定装置の動作を詳しく説明する。 図3は、上記進行方向推定装置の動作を示すフローチャートである。図3に示されるように、本進行方向推定装置はステップS1において前処理を実行するが、図4を参照しつつ本処理を詳しく説明する。

図4に示されるように、ステップS11において重力方向ベクトル推定部5が、上記のように重力方向ベクトルを推定する。次にステップS12において、(加速度成分)重力方向ベクトル射影計算部7が、加速度ベクトルを重力方向ベクトルに射影した成分を求める。 次にステップS13においてバッファ8が、ステップS12で得られた成分をバッファに格納する。次に、ステップS14において1次元FFT演算部9が、ステップS13においてバッファ8に格納されたデータに対して1次元FFT演算を適用する。そして、ステップS15において歩行周波数・位相推定部10が、ステップS14の演算により得られたデータ(FFTデータ)から歩行周波数とその位相を求める。

以上の前処理を終えた後に、本進行方向推定装置は、図3に示されたステップS2において進行方向ベクトル決定部50が進行方向を数式から算出するが、図5を参照しつつ本算出の方法を詳しく説明する。なお、図5に示される動作は全て進行方向ベクトル決定部50により実行される。

図5に示されるように、ステップS21において重力方向ベクトル推定部5から推定された重力方向ベクトルを取得する。次に、ステップS22においてステップS21において取得された重力方向ベクトルに直交する一つの単位ベクトルを水平基準方向ベクトルと決定する。次に、ステップS23において目的関数を設定し、かかる関数において重み係数を決定するが、本ステップにおける動作を以下において詳しく説明する。

まず、進行方向を推定するために以下の式(1)で示される目的関数U(θ)を設定する。

ここで、式(1)における右辺第一項のサブ目的関数Ua(θ)は、以下の式(2)で示される。

また、上記式(1)の右辺第二項のサブ目的関数Uω(θ)は、以下の式(3)で示される。

上記の式(2)及び式(3)において、(v_x,v_y,v_z)は重力方向ベクトル(g_x,g_y,g_z)に直交する進行方向ベクトルを表し、これらの位置関係が図6に示される。

なお、図6に示されるように、人間の歩行の特性上から進行方向ベクトルと重力方向ベクトルは直交すると仮定でき、水平基準方位ベクトル(h_x,h_y,h_z)は重力方向ベクトルに直交する単位ベクトルのうちで任意に選択されたものとされる。

また、上記式(2)及び式(3)におけるω_bとω_hは、それぞれ歩行周波数に相当する角周波数と、歩行周波数の1/2の周波数に相当する角周波数を表す。また、Aは加速度、Wは角速度を表すと共に、Reは実数部分、Imは虚数部分をそれぞれ示す。

ここで、進行方向ベクトル(v_x,v_y,v_z)の各成分は、ロドリゲスの回転公式により以下の式(4)で記述できる。

また同様に、式(2)及び式(3)において、進行方向ベクトルと直交する側方向ベクトルの成分(v_x(θ+π/2),v_y(θ+π/2),v_z(θ+π/2))は、以下の式(5)で記述できる。

ここで、上記式(4)及び式(5)によれば、進行方向ベクトル及びそれと直交する側方向ベクトルの各成分は、いずれも正弦関数と余弦関数の和で表すことができることが分かる。

一般的に上記目的関数Uとして、移動体の個体差(個人差)や状況変化がある場合においても進行方向を的確に推定できる関数を設定することができるが、以下においては式(1)に示された目的関数U(θ)の設定について詳しく説明する。

上記目的関数U(θ)には、式(1)〜式(3)に示されるように、以下の6つの重み係数が存在する。ここで、6つの重み係数とは、加速度成分についてのサブ目的関数Ua(θ)の重み係数w_aと、角速度成分についてのサブ目的関数Uω(θ)の重み係数w_ωと、加速度成分についてのサブ目的関数Ua(θ)に含まれる進行方向成分(ロール軸周り成分)の重み係数w_f,aと、同側方向(ピッチ軸周り成分)の重み係数w_s,aと、角速度成分についてのサブ目的関数Uω(θ)に含まれる進行方向成分(ロール軸周り成分)の重み係数w_f,ωと、同側方向(ピッチ軸周り成分)の重み係数w_s,ωとをいう。

通常の歩行動作においては、加速度成分が運動の特徴として支配的になることが知られているため、上記式(1)において加速度成分の重み係数w_aを1とし、角速度成分の重み係数w_ωを0とした上で解析的な解を得ることも考えられる。

このような方法によれば、角速度成分の離散フーリエ変換という計算等を行う必要がなくなるため、進行方向の推定プロセスが簡略化され、その分計算コストを低減することができる。

また、上記重み係数w_f,a及び重み係数w_s,aは、加速度の進行方向成分と側方向成分を正規化した上で加算するための係数であり、例えば小さな振幅しか生じない成分の係数を大きく設定することによって、かかる成分の特徴を適切に動作に反映させることができる。同様に、上記重み係数w_f,ω及び重み係数w_s,ωは、角速度の進行方向成分と側方向成分を正規化した上で加算するための係数であり、例えば小さな振幅しか生じない成分の係数を大きく設定することによって、かかる成分の特徴を適切に動作に反映させることができる。

次に、進行方向ベクトル決定部50は、図5に示されたステップS24において、1次元FFT演算部19〜21から加速度成分(3軸)の離散フーリエ変換後のデータを取得する。

次に、進行方向ベクトル決定部50は、ステップS25において1次元FFT演算部39〜41から角速度成分(3軸)の離散フーリエ変換後のデータを取得する。そして、ステップS26において、進行方向ベクトル決定部50は、ステップS21で取得された重力方向ベクトルと、ステップS24で取得された加速度成分(3軸)を離散フーリエ変換した結果と、ステップS25で取得された角速度成分(3軸)を離散フーリエ変換した結果と、歩行周波数・位相推定部10から供給された重力方向の加速度成分に基づく歩行周波数の推定結果及び重力方向の加速度成分を離散フーリエ変換した結果の歩行周波数成分の位相とに基づいて、以下の式(8)を用いて目的関数U(θ)が最大となる−180度〜180度の範囲における二つの解を解析的に算出する。

具体的には、上記のように進行方向ベクトルと側方向ベクトルの各成分がいずれも正弦関数と余弦関数の和で記述できることから、式(1)で示された目的関数U(θ)も以下の式(6)のように表すことができることに着目する。

ここで、上記式(6)は、パラメータであるθが以下の式(7)の関係を満たすときに最大値(a²+b²)となる。

式(7)をθにつき解くと、以下の式(8)が得られる。

式(8)を−π<θ≦πの制約条件下で解くとθについて二つの解が得られるが、その二つの解が移動体の進行する向きとその逆の向きを表すことになる。

このことから、上記の式(6)における余弦関数及び正弦関数の各係数a,bを算出することによって、式(8)を用いて上記二つの解を算出する。

しかし、上記のように進行方向の解が−180度〜180度の範囲内であると制限した場合でも、式(8)の解は二つ存在するため、一意に進行方向を決定することはできない。すなわち、上記のように一方の解は正しい進行方向(進行の向き)を示し、もう一方の解はその逆方向(逆の向き)を示したものとなる。

そこで、進行方向ベクトル決定部50は、図3に示されたステップS3において、歩行周波数・位相推定部10から供給された重力方向に射影された加速度成分の歩行周波数成分の位相とステップS2において算出された解とから、進行方向に射影された加速度成分の歩行周波数成分の位相を求め、本位相と上記重力方向に射影された加速度成分の歩行周波数成分の位相との差が所定の数値範囲に入るか否かを検証することにより、当該数値範囲に入るθを正しい進行方向を示す唯一の解として選択する。ここで、上記「所定の数値範囲」とは、移動体による移動が人の歩行動作の場合には概ね−45度〜135度程度の範囲とすべきことが経験的に知られている。 なお、上記位相差は、両成分の相互相関関数を求める方法により算出することもできる。

次に、進行方向ベクトル決定部50は、ステップS4において、ステップS3で算出された目的関数Uが最大となる進行方向を最終的な進行方向として決定する。

具体的には、進行方向ベクトル決定部50は、ステップS3において唯一の解として決定された解を上記式(4)に代入することによって進行方向ベクトルを算出する。

なお、進行方向ベクトル決定部50は、進行方向ベクトルについての情報を必要とする他の回路や装置等(図示していない。)へ、上記の方法により算出された進行方向ベクトルを供給する。

ここで、解析的に得られる目的関数Uの最大値は、その方位角の信頼性の評価に用いることができる。すなわち、当該目的関数Uが十分に大きな最大値を取る場合には方位角の信頼性が高いと評価でき、それ以外の場合には信頼性が低いとみなすことができる。

以上より、本発明の実施の形態に係る進行方向推定装置及び進行方向推定方法によれば、移動体が進行する向き(進行方向)を簡易に推定することができるため、従来よりも効率的に、すなわち計算コストを低減し、かつ高速に進行方向を推定することができる。

なお、本発明の実施の形態に係る進行方向推定装置及び進行方向推定方法は、当該装置が腕を振りながら歩行する人の手に握られた場合に限らず、歩行する人の下肢のポケットに入れられた場合や、歩行する人が持つハンドバックの中に入れられた場合等においても、上記表1に示された特性が得られることから、本発明の効果を得ることができる。

1 加速度センサ(3軸) 3 角速度センサ(3軸) 9、19〜21、39〜41 1次元FFT演算部 50 進行方向ベクトル決定部