JP6548305B2

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

Info

Publication number: JP6548305B2
Application number: JP2015556831A
Authority: JP
Inventors: 興梠　正克; 正克興梠; 蔵田　武志; 武志蔵田
Original assignee: SITESENSING, INC.; National Institute of Advanced Industrial Science and Technology AIST
Current assignee: SITESENSING, INC.; National Institute of Advanced Industrial Science and Technology AIST
Priority date: 2014-01-08
Filing date: 2015-01-08
Publication date: 2019-07-24
Anticipated expiration: 2035-01-08
Also published as: US20170023604A1; JPWO2015105146A1; WO2015105146A1; US10317423B2

Description

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

本発明の実施の形態に係る進行方向推定装置の構成を示すブロック図である。図１に示された１次元ＦＦＴ演算部９により得られた加速度成分のパワースペクトラムの一例を示す図である。図１に示された進行方向推定装置の動作を示すフローチャートである。図３に示されたステップＳ１における前処理の動作を示すフローチャートである。図３に示されたステップＳ２における算出方法を示すフローチャートである。水平基準方位ベクトルと進行方向ベクトルと重力方向ベクトルの位置関係を示す図である。

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

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

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

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

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

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

バッファ１５は、加速度成分(X)測定部１１及び重力方向ベクトル推定部５から出力されたデータを格納する１次元バッファである。バッファ１６は、加速度成分(Y)測定部１２及び重力方向ベクトル推定部５から出力されたデータを格納する１次元バッファである。バッファ１７は、加速度成分(Z)測定部１３及び重力方向ベクトル推定部５から出力されたデータを格納する１次元バッファである。バッファ３５は、角速度成分(X)測定部３１から出力されたデータを格納する１次元バッファである。バッファ３６は、角速度成分(Y)測定部３２から出力されたデータを格納する１次元バッファである。バッファ３７は、角速度成分(Z)測定部３３から出力されたデータを格納する１次元バッファである。

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

ここで、図２に示されるパワースペクトラムにおいては、縦軸がパワー［ｍ／ｓ²］とされ、横軸が周波数［Ｈｚ］とされる。そして、後述するように、本パワースペクトラムに基づいて、歩行周波数・位相推定部１０により２Ｈｚが基本周波数と推定される。

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

１次元ＦＦＴ演算部１９は、バッファ１５に格納されたデータに基づいて、定められたサンプルデータ長について１次元離散フーリエ変換演算を実行し、その周波数領域変換されたデータを出力する。同様に１次元ＦＦＴ演算部２０は、バッファ１６に格納されたデータに基づいて、定められたサンプルデータ長について１次元離散フーリエ変換演算を実行し、その周波数領域変換されたデータを出力する。１次元ＦＦＴ演算部２１は、バッファ１７に格納されたデータに基づいて、定められたサンプルデータ長について１次元離散フーリエ変換演算を実行し、その周波数領域変換されたデータを出力する。１次元ＦＦＴ演算部３９は、バッファ３５に格納されたデータに基づいて、定められたサンプルデータ長について１次元離散フーリエ変換演算を実行し、その周波数領域変換されたデータを出力する。１次元ＦＦＴ演算部４０は、バッファ３６に格納されたデータに基づいて、定められたサンプルデータ長について１次元離散フーリエ変換演算を実行し、その周波数領域変換されたデータを出力する。１次元ＦＦＴ演算部４１は、バッファ３７に格納されたデータに基づいて、定められたサンプルデータ長について１次元離散フーリエ変換演算を実行し、その周波数領域変換されたデータを出力する。

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

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

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

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

進行方向ベクトル決定部５０は、歩行周波数・位相推定部１０から出力された歩行周波数及び位相を示すデータと、１次元ＦＦＴ演算部１９〜２１，３９〜４１から出力された各離散フーリエ変換データと、重力方向ベクトル推定部５から出力された重力方向ベクトルとを入力し、後述する方法によって、移動体の進行方向ベクトルを決定する。

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

図４に示されるように、ステップＳ１１において重力方向ベクトル推定部５が、上記のように重力方向ベクトルを推定する。次にステップＳ１２において、(加速度成分)重力方向ベクトル射影計算部７が、加速度ベクトルを重力方向ベクトルに射影した成分を求める。
次にステップＳ１３においてバッファ８が、ステップＳ１２で得られた成分をバッファに格納する。次に、ステップＳ１４において１次元ＦＦＴ演算部９が、ステップＳ１３においてバッファ８に格納されたデータに対して１次元ＦＦＴ演算を適用する。そして、ステップＳ１５において歩行周波数・位相推定部１０が、ステップＳ１４の演算により得られたデータ(ＦＦＴデータ)から歩行周波数とその位相を求める。

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

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

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

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

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

上記の式(２)及び式(３)において、(ｖ_x，ｖ_y，ｖ_z)は重力方向ベクトル(ｇ_x，ｇ_y，ｇ_z)に直交する進行方向ベクトルを表し、これらの位置関係が図６に示される。

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

また、上記式(２)及び式(３)におけるω_bとω_hは、それぞれ歩行周波数に相当する角周波数と、歩行周波数の１／２の周波数に相当する角周波数を表す。また、Ａは加速度、Ｗは角速度を表すと共に、Ｒｅは実数部分、Ｉｍは虚数部分をそれぞれ示す。

ここで、進行方向ベクトル(ｖ_x，ｖ_y，ｖ_z)の各成分は、ロドリゲスの回転公式により以下の式(４)で記述できる。

また同様に、式(２)及び式(３)において、進行方向ベクトルと直交する側方向ベクトルの成分(ｖ_x(θ＋π／２)，ｖ_y(θ＋π／２)，ｖ_z(θ＋π／２))は、以下の式(５)で記述できる。

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

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

上記目的関数Ｕ(θ)には、式(１)〜式(３)に示されるように、以下の６つの重み係数が存在する。ここで、６つの重み係数とは、加速度成分についてのサブ目的関数Ｕａ(θ)の重み係数ｗ_aと、角速度成分についてのサブ目的関数Ｕω(θ)の重み係数ｗ_ωと、加速度成分についてのサブ目的関数Ｕａ(θ)に含まれる進行方向成分(ロール軸周り成分)の重み係数ｗ_f,aと、同側方向(ピッチ軸周り成分)の重み係数ｗ_s,aと、角速度成分についてのサブ目的関数Ｕω(θ)に含まれる進行方向成分(ロール軸周り成分)の重み係数ｗ_f,ωと、同側方向(ピッチ軸周り成分)の重み係数ｗ_s,ωとをいう。

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

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

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

次に、進行方向ベクトル決定部５０は、図５に示されたステップＳ２４において、１次元ＦＦＴ演算部１９〜２１から加速度成分(３軸)の離散フーリエ変換後のデータを取得する。

次に、進行方向ベクトル決定部５０は、ステップＳ２５において１次元ＦＦＴ演算部３９〜４１から角速度成分(３軸)の離散フーリエ変換後のデータを取得する。そして、ステップＳ２６において、進行方向ベクトル決定部５０は、ステップＳ２１で取得された重力方向ベクトルと、ステップＳ２４で取得された加速度成分(３軸)を離散フーリエ変換した結果と、ステップＳ２５で取得された角速度成分(３軸)を離散フーリエ変換した結果と、歩行周波数・位相推定部１０から供給された重力方向の加速度成分に基づく歩行周波数の推定結果及び重力方向の加速度成分を離散フーリエ変換した結果の歩行周波数成分の位相とに基づいて、以下の式(８)を用いて目的関数Ｕ(θ)が最大となる−１８０度〜１８０度の範囲における二つの解を解析的に算出する。

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

ここで、上記式(６)は、パラメータであるθが以下の式(７)の関係を満たすときに最大値(ａ²＋ｂ²)となる。

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

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

このことから、上記の式(６)における余弦関数及び正弦関数の各係数ａ，ｂを算出することによって、式(８)を用いて上記二つの解を算出する。

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

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

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

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

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

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

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

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

１加速度センサ(３軸)
３角速度センサ(３軸)
９、１９〜２１、３９〜４１１次元ＦＦＴ演算部
５０進行方向ベクトル決定部

Claims

移動体が進行する向きを推定する進行方向推定装置であって、
前記進行による前記移動体の加速度を検知する加速度検知手段と、
前記進行による前記移動体の角速度を検知する角速度検知手段と、
前記加速度検知手段により検知された前記加速度及び前記角速度検知手段により検知された前記角速度に基づいて、水平面上で前記進行する向きと直交する向き及び前記進行する向きにおける前記移動体の加速度成分の大きさと、前記直交する向き及び前記進行する向きにおける前記移動体の角速度成分の大きさの少なくとも一方に応じた目的関数を演算し、前記目的関数が最大となる方向を算出すると共に、前記移動体の加速度の鉛直方向成分と前記進行する向きの成分における位相差に基づいて、前記方向のうち前記移動体が進行する向きを決定する進行方向決定手段と、
前記加速度検知手段で検知された前記加速度と前記角速度検知手段で検知された前記角速度を離散フーリエ変換するフーリエ変換手段とを備え、
前記進行方向決定手段は、前記目的関数の演算として、前記フーリエ変換手段により前記加速度の前記進行する向きの成分を離散フーリエ変換して得られる第一の周波数成分の振幅と、前記フーリエ変換手段により前記加速度の前記直交する向きの成分を離散フーリエ変換したときにおける前記第一の周波数の１／２の第二の周波数成分の振幅とに基づいて得られる第一の目的関数と、前記フーリエ変換手段により前記角速度の前記進行する向きの成分を離散フーリエ変換して得られる前記第二の周波数成分の振幅と、前記角速度の前記直交する向きの成分を離散フーリエ変換したときにおける前記第一の周波数成分の振幅とに基づいて得られる第二の目的関数とを重み係数を乗じて加算する進行方向推定装置。
前記第一の目的関数は、前記フーリエ変換手段により前記加速度の前記進行する向きの成分を離散フーリエ変換して得られる第一の周波数成分の振幅と、前記フーリエ変換手段により前記加速度の前記直交する向きの成分を離散フーリエ変換したときにおける前記第二の周波数成分の振幅とを共に二乗した上で、重み係数を乗じて加算することにより得られ、
前記第二の目的関数は、前記フーリエ変換手段により前記角速度の前記進行する向きの成分を離散フーリエ変換して得られる前記第二の周波数成分の振幅と、前記角速度の前記直交する向きの成分を離散フーリエ変換したときにおける前記第一の周波数成分の振幅とを共に二乗した上で、重み係数を乗じて加算することにより得られる、請求項１に記載の進行方向推定装置。
移動体が進行する向きを推定する進行方向推定方法であって、
水平面上で前記進行する向きと直交する向き及び前記進行する向きにおける前記移動体の加速度成分の大きさと、前記直交する向き及び前記進行する向きにおける前記移動体の角速度成分の大きさの少なくとも一方に応じた目的関数を設定する第一のステップと、
前記目的関数が最大値となる前記移動体が進行する方向を算出する第二のステップと、
前記移動体の加速度の鉛直方向成分と前記進行する向きの成分における位相差に基づいて、前記第二のステップで算出された前記進行する方向のうち前記移動体が進行していると推定される前記向きを決定する第三のステップとを有し、
前記目的関数は、前記加速度成分の関数である第一の目的関数と、前記角速度成分の関数である第二の目的関数にそれぞれ重み係数を乗じて加算したものであり、
前記第一の目的関数は、前記加速度の前記進行する向きの成分を離散フーリエ変換して得られる第一の周波数成分の振幅と、前記加速度の前記直交する向きの成分を離散フーリエ変換したときにおける前記第一の周波数の１／２の第二の周波数成分の振幅とに基づいて得られるものであり、
前記第二の目的関数は、前記角速度の前記進行する向きの成分を離散フーリエ変換して得られる前記第二の周波数成分の振幅と、前記角速度の前記直交する向きの成分を離散フーリエ変換したときにおける前記第一の周波数成分の振幅とに基づいて得られるものである進行方向推定方法。
前記第一の目的関数は、前記加速度の前記進行する向きの成分を離散フーリエ変換して得られる第一の周波数成分の振幅と、前記加速度の前記直交する向きの成分を離散フーリエ変換したときにおける前記第一の周波数の１／２の第二の周波数成分の振幅とを共に二乗した上で、重み係数を乗じて加算したものであり、
前記第二の目的関数は、前記角速度の前記進行する向きの成分を離散フーリエ変換して得られる前記第二の周波数成分の振幅と、前記角速度の前記直交する向きの成分を離散フーリエ変換したときにおける前記第一の周波数成分の振幅とを共に二乗した上で、重み係数を乗じて加算したものである、請求項３に記載の進行方向推定方法。