JPWO2014010727A1 - 移動体の進行方向推定装置及び進行方向推定方法 - Google Patents

Abstract

進行方向推定装置は、加速度センサ及び角速度センサの出力に基づいて重力方向ベクトルを推定する重力方向ベクトル推定部２４０７と、進行方向ベクトルの候補である仮進行方向ベクトル生成部２４１３と、加速度成分データを重力方向ベクトルに射影した成分データから歩行周波数を推定する歩行周波数推定部２４２４と、加速度成分データを進行方向ベクトルに射影した成分Ａｆの周波数成分と、加速度成分データを左右方向ベクトルに射影した成分Ａｎの周波数成分と、角速度成分データを進行方向ベクトルに射影した成分Ｗｓの周波数成分と、角速度成分データを左右方向ベクトルに射影した成分Ｗｎの周波数成分と、歩行周波数と、歩行周波数の位相に基づいて目的関数及び位相差を求め、位相差が所定範囲内であり、かつ目的関数の値が最大となる仮進行方向ベクトルを進行方向ベクトルとし、さらに進行方向の向きを決定する進行方向ベクトル決定部２４２５とを有する。

Description

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

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

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

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

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

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

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

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

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

しかし、非特許文献２においては、進行方位の推定アルゴリズムとして、連続的に方位角を必要な分解能だけ変動させることを提案しているのみで、具体的な実現方法が開示されていなかった。

一方、特許文献３の段落［００２３］から段落［００４１］等では、人間の一般的な歩行動作により生じる進行方向などにおける加速度や角速度の特徴的パターンと実際の計測データとを照合することにより、当該歩行動作の進行方向を推定する装置等が開示されており、特許文献４の段落［００３３］では、人の動作を識別するために、加速度と角速度の間の相互相関や周波数領域での位相差に着目する手法が開示されている。

特開２００９−１５６６６０号公報 特開２０１１−２３７４５２号公報 特開２０１１−２４５２８５号公報 特開２０１２−１４５４５７号公報

「歩行者ナビのための自蔵センサモジュールを用いた屋内測位システムとその評価」シンポジウム「モバイル08」予稿集,pp.151-156(2008) 「歩行者デッドレコニングに基づくハンドヘルド端末の屋内外測位技術」信学技報MVE2010-96,pp.171-176(2011)

自蔵センシング装置が移動体に保持された場合には、例えば、人の手に握られて当該人が腕を振りながら歩行した場合等において、自蔵センシング装置の保持位置や移動体の姿勢が変化することにより自蔵センシング装置と移動体との位置関係が可変である場合には、当該装置により移動体の進行方向を正しく推定することが困難であるという問題があった。
本発明は、このような問題を解決するためになされたもので、移動体の進行方向を精度よく推定することのできる装置及び方法を提供することを目的とする。

上記課題を解決するために、本発明に係る進行方向推定装置は、移動体の加速度ベクトル成分データを計測する加速度計測部と、移動体の角速度ベクトル成分データを計測する角速度計測部と、加速度ベクトル成分データ及び角速度ベクトル成分データに基づいて、移動体の重力方向ベクトルを推定する重力方向ベクトル推定部と、重力方向ベクトル推定部が出力した重力方向ベクトルに基づいて仮進行方向ベクトル及び左右方向ベクトルを生成する仮進行方向ベクトル生成部と、加速度成分データから重力方向ベクトルを除いた後の加速度ベクトル成分データを重力方向ベクトルに射影した成分データを算出し、その周波数領域成分を出力することで歩行周波数とその周波数成分の位相を求める歩行周波数算定部と、仮進行方向ベクトルと、重力方向ベクトルと、加速度ベクトル成分データに基づいて、加速度ベクトル成分データから重力加速度ベクトルを除いた後の加速度ベクトル成分データを仮進行方向ベクトルに射影した成分データを算出し、その周波数領域成分を出力する演算部と、重力方向ベクトルと、左右方向ベクトルと、加速度ベクトル成分データに基づいて、加速度ベクトル成分データから重力加速度ベクトルを除いた後の加速度ベクトル成分データを、左右方向ベクトルに射影した成分データを算出し、その周波数領域成分を出力する演算部と、仮進行方向ベクトルと、角速度ベクトル成分データに基づいて、角速度ベクトル成分データを仮進行方向ベクトル上に射影した成分を算出し、その周波数領域成分を出力する演算部と、左右方向ベクトルと、角速度ベクトル成分データに基づいて、角速度ベクトル成分データを左右方向ベクトル上に射影した成分を算出し、その周波数領域成分を出力する演算部と、歩行周波数と、加速度成分進行方向ベクトルの射影した周波数領域成分と、加速度成分左右方向ベクトルの射影した周波数領域成分と、角速度進行方向ベクトルの射影した周波数領域成分と、角速度左右方向ベクトルの射影した周波数領域成分と、に基づいて歩行周波数の周波数成分又は歩行周波数の1/2の周波数成分からパワー値を算出することで、目的関数を求め、加速度成分進行方向ベクトルの射影した周波数領域成分から、歩行周波数の周波数成分の位相と算出して、歩行周波数算定部が算定した歩行周波数の位相との位相差をもとめ、位相差が所定範囲内の場合に、目的関数の値を取る仮進行方向ベクトルを候補とし、候補となった仮進行方向ベクトルのうち、目的関数の値が最大となる仮進行方向ベクトルと進行方向ベクトルとして出力する進行方向ベクトル決定部と、を有することを特徴とする。

また、上記課題を解決するために、本発明に係る進行方向推定方法は、移動体の加速度ベクトル成分データ及び角速度ベクトル成分データを計測する第一のステップと、第一のステップで計測された加速度ベクトル成分データ及び角速度ベクトル成分データに基づいて、移動体の重力方向ベクトルを推定する第二のステップと、第二のステップで推定された重力方向ベクトルに基づいて仮進行方向ベクトル及び左右方向ベクトルを生成する第三のステップと、第一のステップで計測された加速度ベクトル成分データから第二のステップで推定された重力方向ベクトルを除いた後の加速度ベクトル成分データを重力方向ベクトルに射影した成分データを算出し、その周波数領域成分を出力することで歩行周波数とその周波数成分の位相を求める第四のステップと、第三のステップで生成された仮進行方向ベクトルと、第二のステップで推定された重力方向ベクトルと、第一のステップで計測された加速度ベクトル成分データに基づいて、加速度ベクトル成分データから重力加速度ベクトルを除いた後の加速度ベクトル成分データを仮進行方向ベクトルに射影した成分データを算出し、その周波数領域成分を求める第五のステップと、第二のステップで推定された重力方向ベクトルと、第三のステップで生成された左右方向ベクトルと、第一のステップで計測された加速度ベクトル成分データに基づいて、加速度ベクトル成分データから重力加速度ベクトルを除いた後の加速度ベクトル成分データを、左右方向ベクトルに射影した成分データを算出し、その周波数領域成分を求める第六のステップと、第三のステップで生成された仮進行方向ベクトルと、第一のステップで計測された角速度ベクトル成分データに基づいて、角速度ベクトル成分データを仮進行方向ベクトル上に射影した成分を算出し、その周波数領域成分を求める第七のステップと、第三のステップで生成された左右方向ベクトルと、第一のステップで計測された角速度ベクトル成分データに基づいて、角速度ベクトル成分データを左右方向ベクトル上に射影した成分を算出し、その周波数領域成分を求める第八のステップと、第四のステップで求められた歩行周波数と、第五のステップで求められた加速度ベクトル成分データの仮進行方向ベクトル上に射影した周波数領域成分と、第六のステップで求められた加速度ベクトル成分データの左右方向ベクトル上に射影した周波数領域成分と、第七のステップで求められた角速度ベクトル成分データの仮進行方向ベクトル上に射影した周波数領域成分と、第八のステップで求められた角速度ベクトル成分データの左右方向ベクトル上に射影した周波数領域成分と、に基づいて歩行周波数の周波数成分又は歩行周波数の1/2の周波数成分からパワー値を算出することで、目的関数を求め、加速度ベクトル成分データの進行方向ベクトル上に射影した周波数領域成分から、歩行周波数の周波数成分の位相を算出して、歩行周波数算定部が算定した歩行周波数の位相との位相差を求め、位相差が所定範囲内の場合に、目的関数の値を取る仮進行方向ベクトルを候補とし、候補となった仮進行方向ベクトルのうち、目的関数の値が最大となる仮進行方向ベクトルを進行方向ベクトルと決定する第九のステップとを有することを特徴とする。

本発明の進行方向推定装置及び進行方向推定方法によれば、自蔵センシング装置の保持位置や姿勢が変化することにより、自蔵センシング装置と移動体との位置関係が可変である場合であっても、精度よく進行方向を推定することができる。

本発明の第一の実施の形態に係る移動体の進行方向推定装置の基本的な構成を示すブロック図である。 重力方向の加速度成分Agのパワースペクトラムを示す図である。 仮進行方向の加速度成分Afのパワースペクトラムを示す図である。 仮進行方向を軸とする角速度成分Wsのパワースペクトラムを示す図である。 左右方向の軸(画面軸)周りの角速度成分Wnのパワースペクトラムを示す図である。 図１に示された進行方向ベクトル決定部の構成の一例を示すブロック図である。 本発明の第一の実施の形態における移動体の進行方向推定装置が進行方向を推定するために行う処理を示すフローチャートである。 図３のステップS500の処理の内容を示すフローチャートである。 図１に示された進行方向推定装置の動作を示すフローチャートである。 進行方向推定装置が腕を振りながら歩行する人の手に握られた場合の加速度成分Ag,Af及び角速度成分Wnの時間軸上における位相関係を示すグラフである。 本発明の第二の実施の形態に係る移動体の進行方向推定装置の基本的な構成を説明する機能ブロック図である。 本発明の第二の実施の形態における移動体の進行方向推定装置が進行方向を推定するために行う処理を示すフローチャートである。

（第一の実施の形態）
以下、本発明を実施するための第一の実施の形態について、図面を参照して説明する。
図１は、本発明の第一の実施の形態に係る移動体の進行方向推定装置の基本的な構成を説明する機能ブロック図である。

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

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

重力方向ベクトル推定部2407は、加速度成分(３軸)と角速度成分(３軸)を入力として、移動体の重力方向ベクトルを推定して出力する処理部である。このような処理は、従来から行われている手法によって実現可能である。

仮進行方向ベクトル生成部2413は、重力方向ベクトル推定部2407の出力をその入力として、その重力方向ベクトルに直交する進行方向ベクトルの候補である仮進行方向ベクトル群を必要な分解能に応じて生成する処理部である。人の歩行移動による進行方向ベクトルはほとんどの場合、歩行する平面上に平行であるため、進行方向ベクトルはこの重力方向ベクトルに直交しているとみなせる。そこで、重力方向ベクトルに基づいて、鉛直方向に直交している進行方向を候補として任意に一つ取り出すことができる。仮進行方向ベクトル生成部2413は、たとえば、0.5度刻みの分解能が必要である場合、360度／0.5度＝720通りの仮進行方向ベクトルが生成する。

また、仮進行方向ベクトルを決めると、その方向ベクトルと鉛直方向(重力方向)の両方に直交している方向ベクトル(単位ベクトル)を決定することができ、これを仮左右方向ベクトルとする。仮進行方向ベクトル生成部2413は、進行方向ベクトルが一つ生成すると、重力方向ベクトルとの外積ベクトルを計算することによって、左右方向ベクトルについても同時に生成する。つまり、仮進行方向ベクトルが720通り生成される場合、対応する左右方向ベクトルも720通り生成される。

(加速度成分)重力方向ベクトルAg射影計算部2414は、加速度ベクトル(３軸)と重力方向ベクトル推定部2407の出力を入力として、加速度ベクトルのうち重力加速度ベクトル(＝1G)を差し引いた後の運動加速度ベクトルのうち、重力方向ベクトルに射影される成分の大きさを計算して出力する。

(加速度成分)進行方向ベクトルAf射影計算部2415は、加速度ベクトル(３軸)と重力方向ベクトル推定部2407の出力、仮進行方向ベクトル生成部2413の出力をその入力として、加速度ベクトルから重力加速度ベクトルを除去した後の運動加速度ベクトルを、仮進行方向ベクトル生成部2413の出力する進行方向ベクトルに射影した成分の大きさを計算して出力する。

(加速度成分)左右方向ベクトルAn射影計算部2416は、加速度ベクトル(３軸)と重力方向ベクトル推定部2407の出力、仮進行方向ベクトル生成部2413の出力をその入力として、加速度ベクトルから重力加速度ベクトルを除去した後の運動加速度ベクトルを、仮進行方向ベクトル生成部2413の出力する左右方向ベクトルに射影した成分の大きさを計算して出力する。

(角速度成分)進行方向ベクトルWs射影計算部2417は、角速度ベクトル(３軸)と仮進行方向ベクトル生成部2413の出力をその入力として、角速度ベクトルをその進行方向ベクトルに射影した成分の大きさを計算し、出力する。

(角速度成分)左右方向ベクトルWn射影計算部2418は、角速度ベクトル(３軸)と仮進行方向ベクトル生成部2413の出力をその入力として、角速度ベクトルをその左右方向ベクトルに射影した成分の大きさを計算し、出力する。

具体的には、(角速度成分)左右方向ベクトルWn射影計算部2418は、以下の［数１］から［数３］に示された三式のいずれかにより、左右方向の角速度成分Wnを算出する。

なお、上記［数１］から［数３］までの三式において、gベクトルは重力方位ベクトルを表し、成分(g_x,g_y,g_z)を有するものである。また、ωべクトルは角速度ベクトルを表し、成分(ω_x,ω_y,ω_z)を有するものである。

ここで、左右方向の角速度成分Wnを上記三式のいずれによって算出するかについては、進行方向推定装置のセンサ座標系において、進行動作期間中にその左右方向がX,Y,Z軸のうち最も接近している軸を利用して計算することによって決定する。すなわち、たとえばZ軸が最も近接していることが事前に分かっている場合には、［数３］に示された式を用いて左右方向の角速度成分Wnを算出する。

ただし、重力方位ベクトルがその事前に決定した軸方向に接近した場合、すなわちベクトルv_x,y,zの大きさが一定の閾値以下となるほど小さい場合、その代替軸を事前に決めておき、その軸を使った成分で再計算する。

まとめると、たとえば、事前に決めた軸がZ軸で、代替軸がX軸である場合、ベクトルv_zの大きさが一定の閾値以上であれば、［数３］に示された式によって左右方向の角速度成分Wnを算出し、その閾値を下回った場合は、［数１］に示された式によって左右方向の角速度成分Wnを算出する。

具体的な例を挙げて説明すると、進行方向推定装置のセンシング手段として、スマートフォンなどが使われるが、この場合、人によるスマートフォンの通常の保持のされ方では、スマートフォンが有する主面(画面)に垂直な軸(これをZ軸とする)が腕をスイングさせる回転軸と一致することが多い。そこで、前述した事前に決めた軸はZ軸となる。

ただし、画面を下に向けて保持してスイングすることもあり得るが、その場合でも画面の短辺を片手で保持してスイングさせることが最もあり得るパターンであるため、短辺の軸が代替軸となり、これをX軸と決めれば良いと分かる。

バッファ2430は、(加速度成分)重力方向ベクトル射影計算部2414の出力を格納する１次元バッファである。バッファリングはFIFOによって実現されることが一般的ではあるが、格納された時間順が保持されるバッファであれば十分である。以下のバッファ2430〜2434についても、同様である。

バッファ2431は、(加速度成分)進行方向ベクトル射影計算部2415の出力を格納する１次元バッファである。バッファ2432は、(加速度成分)左右方向ベクトル射影計算部2416の出力を格納する１次元バッファである。バッファ2433は、(角速度成分)進行方向ベクトル射影計算部2417の出力を格納する１次元バッファである。バッファ2434は、(角速度成分)左右方向ベクトル射影計算部2418の出力を格納する１次元バッファである。

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

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

１次元FFT演算部2420は、バッファ2431に格納された加速度成分データに基づいて、定められたサンプルデータ長について、１次元離散フーリエ変換演算を実行して、その周波数領域変換されたデータを出力する装置である。

なお、本データの一例が図３に示される。図３に示されるように、進行方向の加速度成分Afのパワースペクトラムの横軸は周波数［Hz］、縦軸は単位のないパワーであり、人の歩行が持つ基本的な周波数(基本周波数)の1/2の周波数である1Hzに本スペクトラムのピークが位置していることが分かる。

１次元FFT演算部2421は、バッファ2432に格納された加速度成分データに基づいて、定められたサンプルデータ長について、１次元離散フーリエ変換演算を実行して、その周波数領域変換されたデータを出力する装置である。

１次元FFT演算部2422は、バッファ2433に格納された角速度成分データに基づいて、定められたサンプルデータ長について、１次元離散フーリエ変換演算を実行して、その周波数領域変換されたデータを出力する装置である。なお、本データの一例が図４に示される。

１次元FFT演算部2423は、バッファ2434に格納された角速度成分データに基づいて、定められたサンプルデータ長について、１次元離散フーリエ変換演算を実行して、その周波数領域変換されたデータを出力する装置である。なお、本データの一例が図５に示される。

歩行周波数推定部2424は、１次元FFT演算部2419の出力に基づいて、歩行動作の基本周波数を推定して、その歩行周波数(基本周波数)を出力する。歩行動作の基本周波数を推定する方法は、基本的には、一般的な人の歩行動作の周期(0.5秒〜1.5秒につき１歩程度、つまり0.7〜3.0Hz)の範囲内に特徴的な周波数ピークが存在する場合、その周波数を歩行周波数として決定できる。また、時間領域上で歩行動作を一歩単位で検出することで、その歩行周波数の概算を求め、周波数領域上での対応する周波数ピークを検出することで、その時間間隔から歩行周波数を決定してもよい。時間領域上で歩行動作を検出する方法については、たとえば、特許第４２４３６８４号公報に開示されている。歩行周波数を推定すると、その周波数ピークについて、位相H0を求めて、歩行周波数とともに出力する。

なお、上記において、１次元FFT演算部2422と１次元FFT演算部2423とにより離散フーリエ変換部(角速度３軸分)10が構成され、１次元FFT演算部2419と１次元FFT演算部2420と１次元FFT演算部2421と歩行周波数推定部2424とにより離散フーリエ変換部(加速度３軸分)11が構成される。

位相差(Wn-Ag)算出部2435は、(角速度成分)左右方向ベクトルWn射影計算部2418の出力と(加速度成分)重力方向ベクトルAg射影計算部2414の出力に基づいて、これらの時間軸における位相差を算出する。

位相差(Wn-Af)算出部2436は、(角速度成分)左右方向ベクトルWn射影計算部2418の出力と(加速度成分)進行方向ベクトルAf射影計算部2415の出力に基づいて、これらの時間軸における位相差を算出する。

進行方向ベクトル決定部2425は、歩行周波数推定部2424の出力である歩行周波数と、１次元FFT演算部2420〜2423の出力と、位相差(Wn-Ag)算出部2435及び位相差(Wn-Af)算出部2436の出力をそれぞれ入力する。まず、進行方向ベクトル決定部2425は、図３に示された１次元FFT演算部2420の出力に基づいて、加速度ベクトルの進行方向成分の歩行周波数のパワー値P1と位相H1を求める。また、１次元FFT演算部2421の出力に基づいて、加速度ベクトルの左右方向成分の歩行周波数×1/2(歩行周波数の半分の周波数)のパワー値P2と位相H2を求め、図４に示された１次元FFT演算部2422の出力に基づいて、角速度ベクトルの進行方向成分の歩行周波数×1/2のパワー値P3と位相H3を求め、図５に示された１次元FFT演算部2423の出力に基づいて、角速度ベクトルの左右方向成分の歩行周波数のパワー値P4と位相H4を求める。

進行方向ベクトル決定部2425は、これらの４つのパワー値について、それらを総合的に評価する目的関数U(P1,P2,P3,P4)を定めて、その目的関数を最大化する仮進行方向ベクトルを求める。

つまり、仮進行方向ベクトルと仮左右方向ベクトルが決められた場合、加速度ベクトルをそれぞれの方向に射影して得られる成分について、表１の通り、仮進行方向ベクトルの成分を１次元離散フーリエ変換などにより周波数領域に変換したときに現れる歩行周波数のパワー値と、仮左右方向の成分を周波数領域に変換したときに得られる歩行周波数×1/2のパワー値がそれぞれ最大となるはずである。

例えば、図３に示されるように、仮進行方向ベクトルAfの成分を１次元離散フーリエ変換などにより周波数領域に変換したときに現れる歩行周波数のパワースペクトラムは仮進行方向によって大きさが異なり、進行方向ベクトル決定部2425は大きさが最大となる仮の進行方向を正しい進行方向と推定することになる。なお、図３では、例として正しい進行方向と推定した方向からのずれ角度が0度、30度、60度、90度のグラフのみを示している。

一方、角速度ベクトルを仮進行方向ベクトルと仮左右方向ベクトルに射影して得られる成分については、仮進行方向ベクトルには歩行周波数×1/2のパワー値と、仮左右方向ベクトルには歩行周波数のパワー値がそれぞれ最大となるはずである。

例えば、図４に示されるように、角速度ベクトルを仮進行方向ベクトルに射影して得られる角速度成分Wsのパワースペクトラムは仮進行方向により大きさが異なり、進行方向ベクトル決定部2425は大きさが最大となる仮の進行方向を正しい進行方向であると推定することになる。なお、図４では、例として正しい進行方向と推定した方向からのずれ角度が0度、30度、60度、90度のグラフのみを示している。

このようにして、上記４つの成分について最大化されるべきパワー値が得られる。従って、それぞれのパワー値の重み付き総和を取ることで、最適化すべき目的関数を算出することができる。

目的関数の一例として、たとえば、U(P1,P2,P3,P4)=w1P1+w2P2+w3P3+w4P4などが考えられる。なお、w1、w2、w3、w4は、重み係数である。このような目的関数を最大化させる解は二つあり、正しい進行方向α度とα＋180度の二つが現れる。そこで加速度ベクトルもしくは角速度ベクトルの位相差(成分間の位相差)に基づいて、得られた二つの解のどちらが真の解であるかを検証可能である。位相差として利用可能な組み合わせは、たとえば、左右方向の角速度成分Wnと重力方向の加速度成分Agや、左右方向の角速度成分Wnと進行方向の加速度成分Afが挙げられる。

進行方向ベクトル決定部2425は、たとえば、左右方向の角速度成分Wnと重力方向の加速度成分Agの間の位相差が、おおむね0〜100度の範囲内におさまる解を進行する向きとして最終決定するが、向きの決定については後に詳しく説明する。

図６は、図１に示された進行方向ベクトル決定部2425の構成の一例を示すブロック図である。図６に示されるように、進行方向ベクトル決定部2425は、進行方向の加速度成分Afを生成する加速度成分(Af)生成部14と、進行方向の軸周りの角速度成分Wsを算出する角速度成分(Ws)生成部15と、進行方向選択部16と、前後方向決定部17と、進行方向決定部18とを含む。

ここで、加速度成分(Af)生成部14は仮進行方向ベクトル生成部2413、離散フーリエ変換部11、位相差(Wn-Af)算出部2436、及び進行方向選択部16に接続され、角速度成分(Ws)生成部15は仮進行方向ベクトル生成部2413、離散フーリエ変換部10、及び進行方向選択部16に接続される。また、進行方向選択部16は進行方向決定部18に接続される。また、前後方向決定部17は位相差(Wn-Ag)算出部2435、位相差(Wn-Af)算出部2436、及び進行方向決定部18に接続される。

図７は、本発明の第一の実施の形態における移動体の進行方向推定装置が進行方向を推定するために行う処理を説明するフローチャートである。まず、進行方向を推定するために行う処理の前処理として必要な基礎データの計算処理が行われる(ステップS500)。詳細は、図８を用いて後述する。

仮進行方向ベクトル生成部2413は、推定すべき分解能(たとえば0.5度単位)と重力方向ベクトルが与えられ、それに基づいて重力方向ベクトルに直交する進行方向ベクトルの候補をその分解能刻みに生成し、その候補(仮進行方向ベクトル)の一つを順番に出力する(ステップS501)。

(角速度成分)進行方向ベクトル射影計算部2417は、S501で得られた仮進行方向ベクトル上に角速度ベクトル(３軸)を射影してその成分の大きさを取得する(ステップS502)。その射影された成分は、バッファ2433に格納された後に、１次元FFT演算部2422によって周波数領域上のデータに変換される。

進行方向ベクトル決定部2425は、ステップS502で算出された１次元離散フーリエ変換演算処理後のデータに基づいて、歩行周波数f0×1/2の周波数成分のパワー値P3と位相H3を求める(ステップS503)。なお、歩行周波数f0は、前処理であるステップS500で算出されており、その値を用いる。

(角速度成分)進行方向ベクトル射影計算部2418は、角速度ベクトルを仮進行方向ベクトルと重力方向ベクトルの外積演算で求まる左右方向ベクトル(ピッチ軸)に射影して、その成分の大きさを求める(ステップS504)。さらに、その射影された成分は、バッファ2434に格納された後に、１次元FFT演算部2423によって１次元離散フーリエ変換演算処理を実行して、周波数領域上へデータを変換する。

進行方向ベクトル決定部2425は、ステップS504で変換された１次元離散フーリエ変換データに基づいて、ステップS500で計算された歩行周波数f0を取得し、その歩行周波数の周波数成分のパワー値P4と位相H4を求める(ステップS505)。

(加速度成分)進行方向ベクトル射影計算部2415は、加速度ベクトル(３軸)をステップS501で与えられた仮進行方向ベクトル上に射影し、その成分の大きさを求める(ステップS506)。その射影された成分に対して、バッファ2431に格納された後に、１次元FFT演算部2420によって１次元離散フーリエ変換演算処理を実行して、周波数領域上へデータを変換する。

進行方向ベクトル決定部2425は、ステップS506で変換された１次元離散フーリエ変換データに基づいて、ステップS500で計算された歩行周波数に基づいて歩行周波数の周波数成分のパワー値P1と位相H1を求める(ステップS507)。なお、十分に高いピーク値がこの範囲内に含まれないときは、歩行動作が検出されなかったと見なして、この処理をスキップしてもよい。

(加速度成分)左右方向ベクトル射影計算部2416は、加速度ベクトル(３軸)を、ステップS501で与えられた仮進行方向ベクトルと重力方向ベクトルの外積演算で求まる左右方向ベクトル(ピッチ軸)に射影して、その成分の大きさを求める(ステップS508)。その射影された成分は、バッファ2432に格納された後に、１次元FFT演算部2421によって１次元離散フーリエ変換演算処理を実行して、周波数領域上へデータを変換する。

進行方向ベクトル決定部2425は、ステップS508で変換された１次元離散フーリエ変換データに基づいて、ステップS500で計算された歩行周波数に基づいて歩行周波数×1/2の周波数成分のパワー値P2と位相H2を求める(ステップS509)。

進行方向ベクトル決定部2425は、ステップS503とステップS505、ステップS507、ステップS509で求められたそれぞれのパワー値を引数とする目的関数U(P1,P2,P3,P4)を算出する(ステップS510)。目的関数の一例として、U(P1,P2,P3,P4)=w1P1+w2P2+w3P3+w4P4などが考えられる。このような目的関数を取った場合、その重み係数としてw1=w2=0.2とw3=w4=0.8の組み合わせを取ることが望ましい。但し、これらの重み係数の組み合わせに限らない。

進行方向ベクトル決定部2425は、ステップS507で求められた位相H1とステップS500で求められた位相H0との位相差H1−H0を求め、この位相差が一定の範囲にあるかテストする(ステップS511)。この位相差が一定の範囲内に入る場合のみ、ステップS501で与えられた仮進行方向ベクトルが候補として受け入れ可能であると判定する。ここで位相差の範囲の具体的な値については、外部の記憶装置に記録されている数値を使う構成にしてもよい。位相差の範囲は、例えば、0度〜100度の範囲である。

進行方向ベクトル決定部2425は、ステップS501で生成・出力された仮進行方向ベクトルが目的の分解能ですべての組み合わせを網羅したとき、ステップS513へ処理を進め、それ以外の時、ステップS501に処理を進める(ステップS512)。たとえば、0.5度刻みの分解能で進行方向ベクトルを求める場合、720通りがすべて網羅したとき、処理がステップS513に進み、それ以外のときは処理がステップS501に進む。

進行方向ベクトル決定部2425は、ステップS511の位相差テストを通過したすべての組み合わせの仮進行方向ベクトルについて、目的関数Uを最大化するものを選び、それを進行方向ベクトルとして決定する(ステップS513)。ここで、上記のように目的関数Uを最大化させる解は二つあるため、正しい進行の向き及びその180度逆向きを持つ二つの進行方向ベクトルが決定される。

ここで、進行方向ベクトル決定部2425は、図６に示された進行方向選択部16により、仮の進行方向への加速度成分Afのパワースペクトラムが最大となる方向を正しい進行方向と判断するという簡素な方法によって進行方向ベクトルを決定する実施の形態も考えられる。

そして、このような実施の形態においては、進行方向ベクトルの決定精度を高めるため、さらに進行方向の軸周りの角速度成分Wsを利用することが考えられる。具体的には、３軸の角速度センサでセンシングされた進行方向の軸周りの角速度は離散フーリエ変換部10でフーリエ変換され、周波数成分毎のパワースペクトラムが角速度成分(Ws)生成部15に供給されるが、角速度成分(Ws)生成部15では、仮進行方向ベクトル生成部2413により仮に決定された進行方向毎の成分が算出される。このとき、進行方向選択部16は、進行方向の軸周りの角速度成分Wsのパワースペクトラムが最大となる方向も勘案した上で上記の正しい進行方向を判断することになる。

以上より、進行方向選択部16は、上記加速度成分Afと上記角速度成分Wsの双方を利用することにより最も高い精度で進行方向を推定することができることになるが、いずれか一方の成分のみを利用することによっても進行方向を推定することができる。

図８は、図７のステップS500の処理の内訳を説明するフローチャートである。このステップS500は、サブステップS5101〜S5104から成り、その内容は以下の通りである。

重力方向ベクトル推定部2407は、加速度ベクトルと角速度ベクトルを入力として、重力方向ベクトルを算出する(ステップS5101)。(加速度成分)重力方向ベクトル射影計算部2414は、運動加速度ベクトル(加速度ベクトルから重力加速度ベクトルを除去したもの)をステップS5101で求めた重力方向ベクトルに射影して、その成分の大きさを求める(ステップS5102)。ステップS5102で求めた成分をバッファ2430に格納する(ステップS5103)。１次元FFT演算部2419は、ステップS5103によって格納されたデータ列に対して、１次元離散フーリエ変換演算処理を適用し、周波数領域上に変換した離散フーリエ変換データを出力する(ステップS5104)。

歩行周波数推定部2424は、ステップS5104で得られた離散フーリエ変換データに基づいてピーク周波数成分を取り、歩行周波数f0を求める(ステップS5105)。

以下において、上記ステップS513で決定された向きが逆の二つの進行方向ベクトルのうち、移動体の進行する向きを示す進行方向ベクトルを選択する動作について図９を用いて詳しく説明する。

本進行方向推定装置は、ステップS1において、上記図７に示された方法により進行方向を推定する。

次に、本進行方向推定装置は、推定した進行方向のうち、以下の方法により、その向き(前後)を決定する。ここで、向きの決定では、表２に示されるような人間の特性が利用される。

表２に示されるように、本進行方向推定装置が腕を振りながら歩行する人の手に握られた場合には、その握られた手の左右と左右方向の向き、すなわち左右方向が歩行者の体の向き(内側)か否(外側)かによって、角速度成分Wnと各加速度成分Ag,Afの位相差には一定の関係が生じることが経験的に分かっている。

なお、表２に示されるように、本進行方向推定装置が左手に握られ、かつ、画面が内側を向いている場合と、右手に握られ、かつ、画面が外側を向いている場合とは本進行方向推定装置について見ると空間的には平行進行した二つの場所に置かれていることになるため、位相差(Wn-Ag)と位相差(Wn-Af)が持つ特性は一致するものとなっている(以下、「第一の状態」という)。

同様に、本進行方向推定装置が左手に握られ、かつ、左右方向が外側を向いている場合と、右手に握られ、かつ、左右方向が内側を向いている場合とは本進行方向推定装置について見ると空間的には平行進行した二つの場所に置かれていることになるため、位相差(Wn-Ag)と位相差(Wn-Af)が持つ特性は一致するものとなっている(以下、「第二の状態」という)。

より具体的には、例えば本進行方向推定装置が左手に握られ、その左右方向が内側の向きとされている場合には、表２に示されるように、歩行者によらず、位相差(Wn-Ag)は＋90度近辺になり、位相差(Wn-Af)は−90度近辺になる。図１０は、この場合の角速度成分Wnと各加速度成分Ag,Afの位相関係を示したグラフである。ここで、横軸は時間(秒)を表し、縦軸は単位のないパワーを示す。図１０に示されるように、角速度成分Wnは、加速度成分Agより位相がおよそ90度進んでおり、加速度成分Afとの関係では位相がおよそ90度遅れていることが分かる。

以上より、上記第一の状態と第二の状態とはそれぞれ位相差(Wn-Ag)及び位相差(Wn-Af)が異なるため、少なくとも一方を算出してその大きさを表２の値と照らすことにより、本進行方向推定装置がいずれの状態にあるかを判別することができる。

そして、進行方向が推定できている場合において、上記第一と第二の状態を判別することにより、本進行方向推定装置を持った人等の進行体が進行する向きまで推定することができることになる。

以下において、以上の特性を利用して進行方向の向きまで推定する方法を詳しく説明する。
本進行方向推定装置は、ステップS2において、本進行方向推定装置が有する左右方向の軸周りの角速度成分Wnを算出する。ここで、左右方向の軸周りの角速度成分Wnは、角速度センサによりセンシングされた角速度の大きさに基づいて左右方向ベクトルWn射影計算部2418により算出される。なお、上記のように、算出された左右方向の軸周りの角速度成分Wnのパワースペクトラムの例が図５に示される。

次に、本進行方向推定装置は、ステップS3において、重力方向の加速度成分Agを算出する。ここで、重力方向ベクトルAg射影計算部2414は、加速度センサによりセンシングされた加速度の大きさと、重力方向ベクトル推定部2407により推定された重力方向ベクトルに基づいて、重力方向の加速度成分Agを算出する。なお、重力方向ベクトル推定部2407は、角速度センサと加速度センサで得られたデータに基づいて重力方向を推定する。また、上記のように、本重力方向ベクトルAg射影計算部2414により算出された重力方向の加速度成分Agのパワースペクトラムの例が図２に示される。

次に、本進行方向推定装置は、ステップS4において、左右方向の軸周りの角速度成分Wnと重力軸周りの角速度成分Agとの間の位相差(Wn-Ag)を算出する。ここで、位相差(Wn-Ag)は、左右方向ベクトルWn射影計算部2418で算出された左右方向の軸周りの角速度成分Wnと、重力方向ベクトルAg射影計算部2414で算出された重力軸周りの加速度成分Agに基づいて、位相差(Wn-Ag)算出部2435において算出される。

次に、本進行方向推定装置は、ステップS5において、左右方向の軸周りの角速度成分Wnと進行方向の軸周りの加速度成分Afとの間の位相差(Wn-Af)を算出する。ここで、位相差(Wn-Af)は、左右方向ベクトルWn射影計算部2418で算出された左右方向の軸周りの角速度成分Wnと、加速度成分(Af)生成部１４で得られた進行方向の軸周りの加速度成分Afに基づいて、位相差(Wn-Af)算出部2436において算出される。

次に、本進行方向推定装置は、ステップS6において、前後方向決定部17が位相差(Wn-Ag)算出部2435により算出された位相差(Wn-Ag)及び位相差(Wn-Af)算出部2436により算出された位相差(Wn-Af)の少なくとも一方の大きさと表２の値とを比較することにより、本進行推定装置が上記第一の状態と上記第二の状態とのいずれの状態を有するかを判別し、前後方向を決定する。

なお、前後方向決定部17は、位相差(Wn-Ag)と位相差(Wn-Af)のいずれか一方を利用する場合よりも双方を利用した場合の方が、より確実に前後方向を推定することができる。

そして、進行方向決定部18は、進行方向選択部16で選択された進行方向と前後決定方向決定部17により推定された前後方向に応じて、進行方向及びその向きを推定する。
以上より、本発明の実施の形態に係る進行方向推定装置によれば、本進行方向推定装置を持った人等の移動体の進行方向のみならず、その向きまで高精度に推定することができる。

（第二の実施の形態）
次に本発明の第二の実施の形態について説明する。本発明の第二の実施の形態においては、ある一定期間(例えば、128サンプル分＝1.28秒、100Hzの場合)に進行方向が変化していないとみなすことで離散フーリエ変換にかかる計算コストを低減するものである。なお、本実施の形態において、第一の実施の形態と同様の構成にかかる部分については説明を省略する。

図１１は、本発明の第二の実施の形態にかかる移動体の進行方向推定装置の基本的な構成を説明する機能ブロック図である。移動体に関する計測手段として、加速度センサ(３軸)2501、2502、2503と角速度センサ(３軸)2504、2505、2506が直交座標系において計６つのセンシング手段を有する。これらの出力は、各センサの生データである必要はなく、適切に校正された後のデータであっても良い。

重力方向ベクトル推定部2513は、加速度センサによって計測された加速度ベクトル(３軸)と、角速度センサによって計測された角速度ベクトル(３軸)が入力され、重力方向ベクトルを推定して出力する。ここにいう重力方向ベクトルは単位ベクトルであるが、重力加速度の大きさは原則として既知といえるため、重力加速度ベクトルも同時に算出することができる。そこで、重力加速度ベクトルを加速度センサの出力から除去した結果をバッファに格納することによって、移動体の運動加速度ベクトルを各軸のバッファに格納できる。

具体的には、以下のようなバッファ格納処理が実行される。バッファ2507には、加速度成分(X軸)の出力と重力加速度ベクトル(X軸成分)が入力され、加速度成分(X軸)から重力加速度ベクトル(X軸成分)が除かれた加速度成分が格納される。バッファ2508には、加速度成分(Y軸)の出力と重力加速度ベクトル(Y軸成分)が入力され、加速度成分(Y軸)から重力加速度ベクトル(Y軸成分)が除かれた加速度成分が格納される。バッファ2509には、加速度成分(Z軸)の出力と重力加速度ベクトル(Z軸成分)が入力され、加速度成分(Z軸)から重力加速度ベクトル(Z軸成分)が除かれた加速度成分が格納される。

次に、角速度ベクトルについては、角速度センサの３軸の要素各々の出力をそのままバッファに格納する。バッファ2510には、角速度成分(X軸)を格納する。バッファ2511には、角速度成分(Y軸)を格納する。バッファ2512には、角速度成分(Z軸)を格納する。

１次元FFT演算部2531は、運動加速度成分(X軸)を格納しているバッファ2507から得られるデータ列を入力として、１次元FFT演算処理を適用して、周波数領域に変換されたFFTデータを求め、出力する。

１次元FFT演算部2532は、運動加速度成分(Y軸)を格納しているバッファ2508から得られるデータ列を入力として、１次元FFT演算処理を適用して、周波数領域に変換されたFFTデータを求め、出力する。

１次元FFT演算部2533は、運動加速度成分(Z軸)を格納しているバッファ2509から得られるデータ列を入力として、１次元FFT演算処理を適用して、周波数領域に変換されたFFTデータを求め、出力する。

１次元FFT演算部2534は、角速度成分(X軸)を格納しているバッファ2510から得られるデータ列を入力として、１次元FFT演算処理を適用して、周波数領域に変換されたFFTデータを求め、出力する。

１次元FFT演算部2535は、角速度成分(Y軸)を格納しているバッファ2511から得られるデータ列を入力として、１次元FFT演算処理を適用して、周波数領域に変換されたFFTデータを求め、出力する。

１次元FFT演算部2536は、角速度成分(Z軸)を格納しているバッファ2512から得られるデータ列を入力として、１次元FFT演算処理を適用して、周波数領域に変換されたFFTデータを求め、出力する。

(加速度成分)重力方向ベクトル射影計算部2515は、重力方向ベクトル推定部2513が出力する重力方向ベクトルと加速度成分(３軸)を出力する加速度センサ2501，2502，2503の加速度成分を入力として、運動加速度ベクトルの重力方向ベクトルへの射影成分を求めて出力する。この出力は、バッファ2530へ格納される。

バッファ2530へ格納された運動加速度ベクトルの重力方向ベクトルへの射影成分は、１次元FFT演算部2519によって、周波数領域上のFFTデータに変換されて、出力される。

歩行周波数推定部2524は、１次元FFT演算部2519が出力するFFTデータに基づいて、歩行動作の基本周波数を推定する。この推定方法は、前述の実施例に示した方法と同一で良い。

仮進行方向ベクトル生成部2514は、重力方向ベクトル推定部2513の出力である重力方向ベクトルを入力として、その重力方向ベクトルに直交した進行方向ベクトルの候補群を網羅的に生成・出力する。この生成方法については、前述の実施例に示した方法と同様である。

(加速度成分)進行方向ベクトル射影FFT計算部2537は、仮進行方向ベクトル生成部2514が生成・出力した仮進行方向ベクトルと、１次元FFT演算部2531、2532、2533が出力する運動加速度ベクトルの各成分のFFTデータを入力として、運動加速度ベクトルを仮進行方向ベクトルに射影した成分のFFTデータを算出・出力する。なお、このような計算は、進行方向ベクトルがFFTのサンプル時間中に変化しないと仮定した場合にのみ正当化できるものであり、実際には近似計算となっている。１次元FFT演算回数を大幅に低減できることができる。

(加速度成分)左右方向ベクトル射影FFT計算部2538は、仮進行方向ベクトル生成部2514が生成・出力した左右方向ベクトルと、１次元FFT演算部2531、2532、2533が出力する運動加速度ベクトルの各成分のFFTデータを入力として、運動加速度ベクトルを左右方向ベクトル(ピッチ軸)に射影した成分のFFTデータを算出・出力する。

(角速度成分)進行方向ベクトル射影FFT計算部2539は、仮進行方向ベクトル生成部2514が生成・出力した仮進行方向ベクトルと、１次元FFT演算部2534、2535、2536が出力する角速度ベクトルの各成分のFFTデータを入力として、角速度ベクトルを進行方向ベクトルに射影した成分のFFTデータを算出・出力する。

(角速度成分)左右方向ベクトル射影FFT計算部2540は、仮進行方向ベクトル生成部2514が生成・出力した左右方向ベクトルと、１次元FFT演算部2534、2535、2536が出力する角速度ベクトルの各成分のFFTデータを入力として、角速度ベクトルを左右方向ベクトル(ピッチ軸)に射影した成分のFFTデータを算出・出力する。

位相差(Wn-Ag)算出部2542は、(角速度成分)左右方向ベクトルWn射影FFT計算部2540の出力と(加速度成分)重力方向ベクトルAg射影計算部2515で計算された結果をフーリエ変換した１次元FFT演算部2519の出力に基づいて、左右方向の角速度成分Wnと重力方向の加速度成分Agの位相差を算出する。

位相差(Wn-Af)算出部2543は、(角速度成分)左右方向ベクトルWn射影FFT計算部2540の出力と(加速度成分)進行方向ベクトルAf射影FFT計算部2537の出力に基づいて、左右方向の角速度成分Wnと進行方向の加速度成分Afの位相差を算出する。

進行方向ベクトル決定部2541は、歩行周波数推定部2524の出力である歩行周波数と、(加速度成分)進行方向ベクトル射影FFT計算部2537、(加速度成分)左右方向ベクトル射影FFT計算部2538、(角速度成分)進行方向ベクトル射影FFT計算部2539、(角速度成分)左右方向ベクトル射影FFT計算部2540の４つのFFTデータを入力として、進行方向ベクトルを決定して、出力する要素である。その決定方法については、歩行周波数とFFTデータ(４つ)から求めるため、第一の実施の形態と同様である。

図１２は、本発明の第二の実施の形態における移動体の進行方向推定装置が進行方向を推定するために行う処理を説明するフローチャートである。まず、進行方向を推定するために行う処理の前処理として必要な基礎データの計算処理が行われる(ステップS600)。詳細は、第一の実施の形態における図４と同様であるため、説明を省略する。

仮進行方向ベクトル生成部2514では、推定すべき分解能(たとえば0.5度単位)と重力方向ベクトルが与えられ、それに基づいて重力方向ベクトルに直交する進行方向ベクトルの候補群をその分解能刻みに生成し、その候補(仮進行方向ベクトル)の一つを網羅的に順番に出力する(ステップS601)。

(角速度成分)進行方向ベクトル射影FFT計算部2539では、角速度ベクトルの仮進行方向ベクトル上へ射影された成分を周波数領域へ変換されたFFTデータを、角速度ベクトルの各成分の１次元FFT演算の線形結合によって求める(ステップS602)。仮進行方向ベクトル生成部2514が生成・出力した仮進行方向ベクトルと、１次元FFT演算部2534、2535、2536が出力する角速度ベクトルの各成分のFFTデータを入力として算出する。例えば、仮進行方向ベクトル(d_x,d_y,d_z)が与えられたとき、角速度ベクトルを仮進行方向ベクトルに射影した角速度成分のフーリエ変換は、角速度ベクトルの各成分(X/Y/Z軸)のフーリエ変換の線形結合によって得られる。すなわち、下記数式で表される角速度成分

についてのフーリエ変換は、角速度ベクトルのX/Y/Z軸成分のフーリエ変換の線形結合

によって得られる。

この演算処理において、進行方向ベクトルが、FFT演算の取るサンプル時間長だけの間変化しないものと見なせるとき、各成分の１次元FFTデータの線形結合によって、射影成分のFFTデータを得ることができる。実際には、サンプル時間長の時間について進行方向ベクトルがまったく変化しないことはないので、あくまでこれは近似計算となるが、FFT演算回数を大幅に低減できる。

進行方向ベクトル決定部2541では、ステップS602で求めたFFTデータについて、歩行周波数×1/2の周波数成分のパワー値と位相を求める(ステップS603)。

(角速度成分)左右方向ベクトル射影FFT計算部2540は、角速度ベクトルの左右方向ベクトル(ピッチ軸)上へ射影された成分を周波数領域へ変換されたFFTデータを、角速度ベクトルの各成分の１次元FFT演算の線形結合によって求める(ステップS604)。算出方法は、角速度成分進行方向ベクトル射影FFT演算と同様である。

進行方向ベクトル決定部2541は、ステップS604で求めたFFTデータについて、歩行周波数の周波数成分のパワー値と位相を求める(ステップS605)。

(加速度成分)進行方向ベクトル射影FFT計算部2537では、加速度ベクトルの仮進行方向ベクトル上へ射影された成分を周波数領域へ変換されたFFTデータを、加速度ベクトルの各成分の１次元FFT演算の線形結合によって求める(ステップS606)。例えば、仮進行方向ベクトル(d_x,d_y,d_z)が与えられたとき、加速度ベクトルを仮進行方向ベクトルに射影した加速度成分のフーリエ変換は、加速度ベクトルの各成分(X/Y/Z軸)のフーリエ変換の線形結合によって得られる。すなわち、下記数式で表される加速度成分

についてのフーリエ変換は、加速度ベクトルのX/Y/Z軸成分のフーリエ変換の線形結合

によって得られる。

進行方向ベクトル決定部2541は、ステップS606で求めたFFTデータについて、歩行周波数の周波数成分のパワー値と位相を求める(ステップS607)。

(加速度成分)左右方向ベクトル射影FFT計算部2538は、加速度ベクトルの左右方向ベクトル(ピッチ軸)上へ射影された成分を周波数領域へ変換されたFFTデータを、加速度ベクトルの各成分の１次元FFT演算の線形結合によって求める(ステップS608)。算出方法は、加速度成分進行方向ベクトル射影FFT演算と同様である。

進行方向ベクトル決定部2541は、ステップS608で求めたFFTデータについて、歩行周波数×1/2の周波数成分のパワー値と位相を求める(ステップS609)。

進行方向ベクトル決定部2541は、前述の実施例と同一の方法により、ステップS603、S605、S607、S609で求められたパワー値に基づいて目的関数の値を算出する(ステップS610)。

進行方向ベクトル決定部2541は、ステップS607で求められた位相とステップS600で求められた加速度ベクトルの重力方向ベクトルへの射影成分の位相との位相差が一定の範囲内に入っていることを検査する(ステップS611)。

進行方向ベクトル決定部2425は、すべての仮進行方向ベクトルについて処理が実行された場合、処理をステップS613に進め、それ以外の場合、ステップS601に処理を進める(ステップS612)。

進行方向ベクトル決定部2541は、ステップS611の検査にパスした進行方向ベクトルの候補のうち、目的関数Uを最大化するものを、最終的な進行方向ベクトルとして互いに向きが逆の二つのベクトルを出力する(ステップS613)。

このように、ある一定期間に進行方向が変化していないとみなせる場合、その期間における離散フーリエ変換(DFT)の結果は、センサ座標系における各成分の離散フーリエ変換の線形結合で表すことができる。このため、第一の実施の形態のように、720通り(0.5度刻み)すべてのサンプルデータを生成し1440回の離散フーリエ変換演算処理を行う必要がなく、計算コストを削減できる。

つまり、その都度離散フーリエ変換を実行しなくとも、センサ座標系における加速度ベクトル・角速度ベクトル３軸(X/Y/Z軸)について、それぞれ離散フーリエ変換を実行し、離散フーリエ変換を適用する直前に得られた重力方向ベクトルと加速度ベクトル、重力方向ベクトルと角速度ベクトルの内積を取ることで、重力方向ベクトルは単位ベクトルなので加速度ベクトルと角速度ベクトルの重力方向への各成分をそれぞれ求めることができる。重力方向ベクトルが変化しないものと見なすと、加速度ベクトルと重力方向ベクトルとの線形結合をフーリエ変換した結果は、加速度ベクトルの各成分をフーリエ変換した結果と重力方向ベクトルとの線形結合に一致するためである。同様に、角速度ベクトルと重力方向ベクトルとの線形結合をフーリエ変換した結果は、角速度ベクトルの各成分をフーリエ変換した結果と重力方向ベクトルとの線形結合に一致する。

従って、進行方向ベクトルの候補が複数通り得られた場合であっても、その処理に要する離散フーリエ変換演算処理の回数は、加速度ベクトルの各成分(X/Y/Z軸)と角速度ベクトルの各成分(X/Y/Z軸)に対する適用のみとるため計６回となり、大幅に低減することが可能となる。

また、進行方向ベクトル決定部2541は、上記第一の実施の形態に係る進行方向ベクトル決定部2425と同様な方法により、上記ステップS613で出力された互いに向きが逆の二つの進行方向ベクトルのうち、移動体の進行する向きを示すベクトルを選択することができる。

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

１０ 離散フーリエ変換部(角速度３軸分)
１１ 離散フーリエ変換部(加速度３軸分)
１６ 進行方向選択部
１７ 前後方向決定部
１８ 進行方向決定部
２４０１、２４０２、２４０３ 加速度成分
２４０４、２４０５、２４０６ 角速度成分
２４０７ 重力方向ベクトル推定部
２４１３ 仮進行方向ベクトル生成部
２４１４ (加速度成分)重力方向ベクトル射影計算部
２４１５ (加速度成分)進行方向ベクトル射影計算部
２４１６ (加速度成分)左右方向ベクトル射影計算部
２４１７ (角速度成分)進行方向ベクトル射影計算部
２４１８ (角速度成分)左右方向ベクトル射影計算部
２４３０〜２４３４ バッファ
２４１９〜２４２３ １次元FFT演算部
２４２４ 歩行周波数推定部
２４２５ 進行方向ベクトル決定部
２４３５，２５４２ 位相差(Wn-Ag)算出部
２４３６，２５４３ 位相差(Wn-Af)算出部

Claims (14)

1. 移動体の進行方向を推定する進行方向推定装置であって、
   移動体の加速度ベクトル成分データを計測する加速度計測部と、
   移動体の角速度ベクトル成分データを計測する角速度計測部と、
   前記加速度ベクトル成分データ及び前記角速度ベクトル成分データに基づいて、移動体の重力方向ベクトルを推定する重力方向ベクトル推定部と、
   前記重力方向ベクトル推定部が出力した重力方向ベクトルに基づいて仮進行方向ベクトル及び左右方向ベクトルを生成する仮進行方向ベクトル生成部と、
   前記加速度成分データから前記重力方向ベクトルを除いた後の加速度ベクトル成分データを重力方向ベクトルに射影した成分データを算出し、その周波数領域成分を出力することで歩行周波数とその周波数成分の位相を求める歩行周波数算定部と、
   前記仮進行方向ベクトルと、前記重力方向ベクトルと、前記加速度ベクトル成分データに基づいて、前記加速度ベクトル成分データから重力加速度ベクトルを除いた後の加速度ベクトル成分データを仮進行方向ベクトルに射影した成分データを算出し、その周波数領域成分を出力する演算部と、
   前記重力方向ベクトルと、前記左右方向ベクトルと、前記加速度ベクトル成分データに基づいて、前記加速度ベクトル成分データから重力加速度ベクトルを除いた後の加速度ベクトル成分データを、前記左右方向ベクトルに射影した成分データを算出し、その周波数領域成分を出力する演算部と、
   前記仮進行方向ベクトルと、前記角速度ベクトル成分データに基づいて、前記角速度ベクトル成分データを前記仮進行方向ベクトル上に射影した成分を算出し、その周波数領域成分を出力する演算部と、
   前記左右方向ベクトルと、前記角速度ベクトル成分データに基づいて、前記角速度ベクトル成分データを前記左右方向ベクトル上に射影した成分を算出し、その周波数領域成分を出力する演算部と、
   前記歩行周波数と、前記加速度成分進行方向ベクトルの射影した周波数領域成分と、前記加速度成分左右方向ベクトルの射影した周波数領域成分と、前記角速度進行方向ベクトルの射影した周波数領域成分と、前記角速度左右方向ベクトルの射影した周波数領域成分と、に基づいて歩行周波数の周波数成分又は歩行周波数の1/2の周波数成分からパワー値を算出することで、目的関数を求め、前記加速度成分進行方向ベクトルの射影した周波数領域成分から、歩行周波数の周波数成分の位相と算出して、前記歩行周波数算定部が算定した歩行周波数の位相との位相差をもとめ、前記位相差が所定範囲内の場合に、目的関数の値を取る仮進行方向ベクトルを候補とし、候補となった仮進行方向ベクトルのうち、目的関数の値が最大となる仮進行方向ベクトルを進行方向ベクトルとして出力する進行方向ベクトル決定部と、を有する進行方向推定装置。
2. 前記歩行周波数算定部は、前記加速度成分データから前記重力方向ベクトルを除いた後の加速度ベクトル成分データを重力方向ベクトルに射影した成分データを算出する加速度成分重力方向ベクトル射影計算部と、
   前記加速度成分データを重力方向ベクトルに射影した成分データに１次元離散フーリエ変換演算を適用して、周波数領域成分を出力する１次元離散フーリエ変換演算部と、
   １次元離散フーリエ変換演算部の出力に基づいて、歩行動作の基本周波数とその周波数成分の位相を求める歩行周波数算定部とを有することを特徴とする請求項１記載の進行方向推定装置。
3. 前記加速度成分進行方向ベクトルの射影した周波数領域成分を出力する演算部は、加速度ベクトル成分データから重力加速度ベクトルを除いた後の加速度ベクトル成分データを仮進行方向ベクトルに射影した成分を計算する加速度成分進行方向ベクトル射影計算部と、前記加速度成分進行方向ベクトル射影計算部が出力した加速度成分に１次元離散フーリエ変換演算を行って周波数領域成分を出力する１次元離散フーリエ変換演算部とを有し、前記加速度成分左右方向ベクトルの射影した周波数領域成分を出力する演算部は、前記重力方向ベクトルと、前記左右方向ベクトルと、前記加速度ベクトル成分データに基づいて、前記加速度ベクトル成分データから重力加速度ベクトルを除いた後の加速度ベクトル成分データを、前記左右方向ベクトルに射影した成分データを算出する加速度成分左右方向ベクトル射影計算部と、前記加速度成分左右方向ベクトル射影計算部が出力した加速度成分に１次元離散フーリエ変換演算を行って周波数領域成分を出力する１次元離散フーリエ変換演算部とを有し、
   前記角速度進行方向ベクトルの射影した周波数領域成分を出力する演算部は、前記仮進行方向ベクトルと、前記角速度ベクトル成分データに基づいて、前記角速度ベクトル成分データを前記仮進行方向ベクトル上に射影した成分を算出する角速度成分進行方向ベクトル射影計算部と、前記角速度成分進行方向ベクトル射影計算部が出力した角速度成分に１次元離散フーリエ変換演算を行って周波数領域成分を出力する１次元離散フーリエ変換演算部とを有し、前記角速度左右方向ベクトルの射影した周波数領域成分を出力する演算部は、前記左右方向ベクトルと、前記角速度ベクトル成分データに基づいて、前記角速度ベクトル成分データを前記左右方向ベクトル上に射影した成分を算出する角速度成分左右方向ベクトル射影計算部と、前記角速度左右方向ベクトル射影計算部が出力した角速度成分に１次元離散フーリエ変換演算を行って周波数領域成分を出力する１次元離散フーリエ変換演算部とを有する、請求項１または２記載の進行方向推定装置。
4. 前記加速度成分進行方向ベクトルの射影した周波数領域成分を出力する演算部は、加速度ベクトル成分データの各々から重力加速度ベクトルを除去した加速度成分に１次元離散フーリエ変換演算を行って出力されたそれぞれの周波数領域成分データと、仮進行方向ベクトルとの線形結合を算出して１次元離散フーリエ変換を行う加速度成分進行方向ベクトル射影離散フーリエ変換計算部であり、
   前記加速度成分左右方向ベクトルの射影した周波数領域成分を出力する演算部は、加速度ベクトル成分データの各々から重力加速度ベクトルを除去した加速度成分に１次元離散フーリエ変換演算を行って出力されたそれぞれの周波数領域成分データと、前記左右方向ベクトルとの線形結合を算出して１次元離散フーリエ変換を行う加速度成分左右方向ベクトル射影離散フーリエ変換計算部であり、
   前記角速度進行方向ベクトルの射影した周波数領域成分を出力する演算部は、角速度ベクトル成分データの各々に１次元離散フーリエ変換演算を行って出力されたそれぞれの周波数領域成分データと、前記仮進行方向ベクトルとの線形結合を算出して１次元離散フーリエ変換を行う角速度成分進行方向ベクトル射影離散フーリエ変換計算部であり、
   前記角速度左右方向ベクトルの射影した周波数領域成分を出力する演算部は、角速度ベクトル成分データの各々に１次元離散フーリエ変換演算を行って出力されたそれぞれの周波数領域成分データと、前記左右方向ベクトルとの線形結合を算出して１次元離散フーリエ変換を行う角速度成分左右方向ベクトル射影離散フーリエ変換計算部である、請求項１又は２記載の進行方向推定装置。
5. 前記角速度ベクトル成分データを前記左右方向ベクトル上に射影した成分と、前記加速度ベクトル成分データを重力方向ベクトルに射影した成分データ及び前記加速度ベクトル成分データを仮進行方向ベクトルに射影した成分データのうち少なくとも一方との間の位相差を算出する位相差算出手段と、
   前記位相差算出手段で算出された少なくとも一つの位相差に応じて、前記進行方向ベクトル決定部により出力された向きが逆の二つの前記進行方向ベクトルのうち前記移動体の進行の向きを示すいずれか一つのベクトルを選択する進行方向選択手段とをさらに備えた請求項１に記載の進行方向推定装置。
6. 前記角速度ベクトル成分データを前記左右方向ベクトル上に射影した成分と前記加速度ベクトル成分データを重力方向ベクトルに射影した成分データとの間の第一の位相差を算出する第一位相差算出手段と、
   前記角速度ベクトル成分データを前記左右方向ベクトル上に射影した成分と前記加速度ベクトル成分データを仮進行方向ベクトルに射影した成分データとの間の第二の位相差を算出する第二位相差算出手段と、
   前記第一及び第二の位相差に応じて、前記進行方向ベクトル決定部により出力された向きが逆の二つの前記進行方向ベクトルのうち前記移動体の進行の向きを示すいずれか一つのベクトルを選択する進行方向選択手段とをさらに備えた請求項１に記載の進行方向推定装置。
7. 前記進行方向選択手段は、
   前記二つの前記進行方向ベクトルの各向きに応じた前記第一及び第二の位相差を基準データとして予め取得し、第一位相差算出手段で算出された前記第一の位相差と前記第二位相差算出手段で算出された前記第二の位相差を前記基準データと比較する請求項６に記載の進行方向推定装置。
8. 移動体の進行方向を推定するための進行方向推定方法であって、
   移動体の加速度ベクトル成分データ及び角速度ベクトル成分データを計測する第一のステップと、
   前記第一のステップで計測された前記加速度ベクトル成分データ及び前記角速度ベクトル成分データに基づいて、移動体の重力方向ベクトルを推定する第二のステップと、
   前記第二のステップで推定された重力方向ベクトルに基づいて仮進行方向ベクトル及び左右方向ベクトルを生成する第三のステップと、
   前記第一のステップで計測された前記加速度ベクトル成分データから前記第二のステップで推定された前記重力方向ベクトルを除いた後の加速度ベクトル成分データを重力方向ベクトルに射影した成分データを算出し、その周波数領域成分を出力することで歩行周波数とその周波数成分の位相を求める第四のステップと、
   前記第三のステップで生成された前記仮進行方向ベクトルと、前記第二のステップで推定された前記重力方向ベクトルと、前記第一のステップで計測された前記加速度ベクトル成分データに基づいて、前記加速度ベクトル成分データから重力加速度ベクトルを除いた後の加速度ベクトル成分データを仮進行方向ベクトルに射影した成分データを算出し、その周波数領域成分を求める第五のステップと、
   前記第二のステップで推定された前記重力方向ベクトルと、前記第三のステップで生成された前記左右方向ベクトルと、前記第一のステップで計測された前記加速度ベクトル成分データに基づいて、前記加速度ベクトル成分データから重力加速度ベクトルを除いた後の加速度ベクトル成分データを、前記左右方向ベクトルに射影した成分データを算出し、その周波数領域成分を求める第六のステップと、
   前記第三のステップで生成された前記仮進行方向ベクトルと、前記第一のステップで計測された前記角速度ベクトル成分データに基づいて、前記角速度ベクトル成分データを前記仮進行方向ベクトル上に射影した成分を算出し、その周波数領域成分を求める第七のステップと、
   前記第三のステップで生成された前記左右方向ベクトルと、前記第一のステップで計測された前記角速度ベクトル成分データに基づいて、前記角速度ベクトル成分データを前記左右方向ベクトル上に射影した成分を算出し、その周波数領域成分を求める第八のステップと、
   前記第四のステップで求められた前記歩行周波数と、前記第五のステップで求められた前記加速度ベクトル成分データの仮進行方向ベクトル上に射影した周波数領域成分と、前記第六のステップで求められた前記加速度ベクトル成分データの左右方向ベクトル上に射影した周波数領域成分と、前記第七のステップで求められた前記角速度ベクトル成分データの仮進行方向ベクトル上に射影した周波数領域成分と、前記第八のステップで求められた前記角速度ベクトル成分データの左右方向ベクトル上に射影した周波数領域成分と、に基づいて歩行周波数の周波数成分又は歩行周波数の1/2の周波数成分からパワー値を算出することで、目的関数を求め、前記加速度ベクトル成分データの進行方向ベクトル上に射影した周波数領域成分から、歩行周波数の周波数成分の位相を算出して、前記歩行周波数算定部が算定した歩行周波数の位相との位相差を求め、前記位相差が所定範囲内の場合に、目的関数の値を取る仮進行方向ベクトルを候補とし、候補となった仮進行方向ベクトルのうち、目的関数の値が最大となる仮進行方向ベクトルを進行方向ベクトルと決定する第九のステップとを有する進行方向推定方法。
9. 前記第四のステップは、
   前記加速度成分データから前記重力方向ベクトルを除いた後の加速度ベクトル成分データを重力方向ベクトルに射影した成分データを算出するステップと、
   前記加速度成分データを重力方向ベクトルに射影した成分データに１次元離散フーリエ変換演算を適用して、周波数領域成分を求めるステップと、
   前記１次元離散フーリエ変換演算の適用による前記周波数領域成分に基づいて、歩行動作の基本周波数とその周波数成分の位相を求めるステップとを含むことを特徴とする請求項８記載の進行方向推定方法。
10. 前記第五のステップは、前記加速度ベクトル成分データから前記重力加速度ベクトルを除いた後の加速度ベクトル成分データを仮進行方向ベクトルに射影した成分を計算して１次元離散フーリエ変換演算を行い周波数領域成分を求めるステップを含み、
    前記第六のステップは、前記重力方向ベクトルと、前記左右方向ベクトルと、前記加速度ベクトル成分データに基づいて、前記加速度ベクトル成分データから重力加速度ベクトルを除いた後の加速度ベクトル成分データを、前記左右方向ベクトルに射影した成分データを算出して１次元離散フーリエ変換演算を行い周波数領域成分を求めるステップを含み、
    前記第七のステップは、前記仮進行方向ベクトルと、前記角速度ベクトル成分データに基づいて、前記角速度ベクトル成分データを前記仮進行方向ベクトル上に射影した成分を算出して１次元離散フーリエ変換演算を行い周波数領域成分を求めるステップを含み、
    前記第八のステップは、前記左右方向ベクトルと、前記角速度ベクトル成分データに基づいて、前記角速度ベクトル成分データを前記左右方向ベクトル上に射影した成分を算出して１次元離散フーリエ変換演算を行い周波数領域成分を求めるステップを含む請求項８記載の進行方向推定方法。
11. 前記第五のステップは、前記加速度ベクトル成分データの各々から前記重力加速度ベクトルを除去した加速度成分に１次元離散フーリエ変換演算を行って出力されたそれぞれの周波数領域成分データと、前記仮進行方向ベクトルとの線形結合を算出して１次元離散フーリエ変換を行うステップを含み、
    前記第六のステップは、前記加速度ベクトル成分データの各々から前記重力加速度ベクトルを除去した加速度成分に１次元離散フーリエ変換演算を行って出力されたそれぞれの周波数領域成分データと、前記左右方向ベクトルとの線形結合を算出して１次元離散フーリエ変換を行うステップを含み、
    前記第七のステップは、前記角速度ベクトル成分データの各々に１次元離散フーリエ変換演算を行って出力されたそれぞれの周波数領域成分データと、前記仮進行方向ベクトルとの線形結合を算出して１次元離散フーリエ変換を行うステップを含み、
    前記第八のステップは、前記角速度ベクトル成分データの各々に１次元離散フーリエ変換演算を行って出力されたそれぞれの周波数領域成分データと、前記左右方向ベクトルとの線形結合を算出して１次元離散フーリエ変換を行うステップを含む請求項８記載の進行方向推定方法。
12. 前記角速度ベクトル成分データを前記左右方向ベクトル上に射影した成分と、前記加速度ベクトル成分データを重力方向ベクトルに射影した成分データ及び前記加速度ベクトル成分データを仮進行方向ベクトルに射影した成分データのうち少なくとも一方との間の位相差を算出する第十のステップと、
    前記第十のステップで算出された少なくとも一つの位相差に応じて、前記第九のステップにより決定された向きが逆の二つの前記進行方向ベクトルのうち前記移動体の進行の向きを示すいずれか一つのベクトルを選択する第十一のステップとをさらに有する請求項８記載の進行方向推定方法。
13. 前記角速度ベクトル成分データを前記左右方向ベクトル上に射影した成分と前記加速度ベクトル成分データを重力方向ベクトルに射影した成分データとの間の第一の位相差を算出する第一位相差算出ステップと、
    前記角速度ベクトル成分データを前記左右方向ベクトル上に射影した成分と前記加速度ベクトル成分データを仮進行方向ベクトルに射影した成分データとの間の第二の位相差を算出する第二位相差算出ステップと、
    前記第一及び第二の位相差に応じて、前記第九のステップで決定された向きが逆の二つの前記進行方向ベクトルのうち前記移動体の進行の向きを示すいずれか一つのベクトルを選択する進行方向選択ステップとをさらに有する請求項８に記載の進行方向推定方法。
14. 前記二つの前記進行方向ベクトルの各向きに応じた前記第一及び第二の位相差を基準データとして予め取得するステップと、
    前記第一位相差算出ステップで算出された前記第一の位相差と前記第二位相差算出ステップで算出された前記第二の位相差を前記基準データと比較するステップとをさらに有する請求項１３に記載の進行方向推定方法。
    
    

Images