JP2015094631A - 位置算出装置及び位置算出方法 - Google Patents

Abstract

【課題】 システムの負荷を抑制でき、しかも角速度の変化が大きい移動体を対象とした場合にも姿勢の算出精度が低下しにくい姿勢算出装置及び姿勢算出方法を提供すること。【解決手段】 姿勢算出装置は、生体または前記生体のエネルギーを動力源として移動する移動体の、慣性座標系に対する第１座標系の角速度を検出するジャイロセンサー１３と、第１周波数ｆ１で入力される角速度に基づいて、移動体の姿勢変化を算出する姿勢変化算出部１１３と、第２周波数ｆ２で入力される姿勢変化に基づいて、移動体の姿勢を更新する姿勢更新部１２４と、含み、第1周波数及び第２周波数は、ｆ１＞ｆ２≧２０Ｈｚを満たす。【選択図】 図３

Description

本発明は、姿勢算出装置及び姿勢算出方法等に関する。

慣性航法システムにおいて、当該システムを搭載した機器や、当該機器を装着した移動体の姿勢を、角速度センサーの出力を用いて算出する手法が知られている。

特許文献１には、比較的高い精度で姿勢角を高速に計算するために、次数が相対的に小さい近似演算により、相対的に大きい演算周波数で姿勢基準計算を行う際の初期値を、次数が相対的に大きい近似演算により、相対的に小さい演算周波数で姿勢基準計算を行った結果を用いて補正することが開示されている。

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

特許文献１では、相対的に小さい演算周波数で姿勢基準演算を行った結果を用いて補正を行っている。演算周波数が小さい演算は、演算周波数が大きい演算よりも小さい周期で入力されたジャイロデータを使用している。そのため、演算に用いないジャイロデータが多く、正しく補正できない可能性があった。

特に、例えば人間のような、短時間における角速度の変化が大きい移動体を対象とした場合には、使用しないデータが多いと誤差が大きく、算出される姿勢角の精度が低下しやすい。一方で、姿勢の算出は演算量が多いため、演算を行うシステムの負荷を軽減したいという要求がある。

本発明の幾つかの態様では、システムの負荷を抑制でき、しかも角速度の変化が大きい移動体を対象とした場合にも姿勢の算出精度が低下しにくい姿勢算出装置及び姿勢算出方法を提供することにある。

（１）本発明の一態様は、生体または前記生体のエネルギーを動力源として移動する移動体（以下、「移動体」と総称する。）の、慣性座標系に対する第１座標系の角速度を検出するジャイロセンサーと、第１周波数ｆ１で入力される前記角速度に基づいて、前記移動体の姿勢変化を算出する姿勢変化算出部と、第２周波数ｆ２で入力される前記姿勢変化に基づいて、前記移動体の姿勢を更新する姿勢更新部と、含み、前記第１周波数及び前記第２周波数は、ｆ１＞ｆ２≧２０Ｈｚを満たす、姿勢算出装置に関する。

本発明の一態様に係る姿勢算出装置は、姿勢変化算出部には角速度が第１周波数ｆ１で入力され、姿勢更新部には姿勢変化が第２周波数で入力される。本願発明者らの検討の結果、ｆ１＞ｆ２≧２０Ｈｚであれば、第２周波数ｆ２よりも高い第１周波数ｆ１で姿勢変化を算出することによって、第２周波数ｆ２で姿勢変化の算出を行う場合よりも精度よく姿勢変化を算出できることが明らかとなった。したがって、歩行運動する移動体のように角速度変化が比較的大きい場合にも、姿勢の算出精度の低下を抑制できる。一方、姿勢更新部には第１周波数ｆ１よりも低い第２周波数ｆ２で姿勢変化が入力される。よって、姿勢更新以降の演算におけるＣＰＵの負荷は低減される。

（２）本発明の一態様では、前記姿勢変化算出部は、ｅｘｐ（［α_ｉｂ ^ｂ（ｔ＋τ_１）］×）ｅｘｐ（［α_ｉｂ ^ｂ（ｔ＋２τ_１）］×）・・・ｅｘｐ（［α_ｉｂ ^ｂ（ｔ＋ｋτ_１）］×）を演算することができる。ただし、τ_１＝１／ｆ１であり、ｋは（ｆ１／ｆ２）−１＜ｋ≦ｆ１／ｆ２を満たす整数であり、ｔは時刻であり、［α_ｉｂ ^ｂ（ｔ＋ｎτ_１）］は時刻ｔ＋ｎτ_１（ただしｎは１≦ｎ≦ｋを満たす整数）における前記角速度から求めた回転行列である。

ｎ番目の第１周期τ_１毎の姿勢変化ｅｘｐ（［α（ｔ＋ｎτ_１）］×）を用い、ｅｘｐ（［α（ｔ＋τ）×］）ｅｘｐ（［α（ｔ＋２τ）×］…ｅｘｐ（［α（ｔ＋ｋτ）］×）のようにｋ回乗算して演算することで、移動体の姿勢変化を精度よく算出することができる。なお、第１，第２周期τ_１，τ_２と上述した第１，第２周波数ｆ１，ｆ２とは、τ_１＝１／ｆ１、τ_２＝１／ｆ２の関係が成立する。よって、ｋ＝ｆ１／ｆ２が成立する。また、ｆ１／ｆ２が整数とならない場合でも、ｋは期間内に掛け算できるα行列の数になるので、（ｆ１／ｆ２）−１＜ｋ≦＝ｆ１／ｆ２を満たす整数ｋとすることができる。

（３）本発明の一態様では、姿勢算出装置は、前記基準座標系に対する前記１座標系の加速度を検出する加速度センサーと、前記姿勢と、前記第２周波数ｆ２で入力される前記加速度とに基づいて、前記第１座標系から第２座標系に前記加速度を座標変換する座標変換部と、をさらに含んでもよい。

姿勢は、第１周波数ｆ１で入力された角速度に基づいて算出された姿勢変化を用いて更新された姿勢である。そのため、当該姿勢を用いることで精度良く加速度を座標変換できる。

（４）本発明の一態様では、前記ジャイロセンサーが収容されるセンサーユニットをさらに含み、前記姿勢変化算出部は、前記センサーユニットに収容されるハードウェアによって構成されてもよい。

姿勢変化を算出する演算は、通常、システムの仕様によらず共通の演算とすることができる。そこで、姿勢変化算出部をハードウェアによって構成することで、相対的に高い第１周波数ｆ１を用いた演算によるＣＰＵの負担を軽減できる。

（５）本発明の一態様では、前記第２周期τ_２（１／ｆ２）の間に第３周波数ｆ３（ｆ２＜ｆ３＜ｆ１）で入力された前記加速度を、ｍ（ｍは２以上の整数で、ｍ＝ｆ３／ｆ２））回加算平均して、前記第２周波数ｆ２にて前記姿勢更新部に入力させる加算平均部をさらに有することができる。

加速度センサーが加速度を検出する周波数は第２周波数ｆ２に一致しても一致しなくても良い。第２周波数ｆ２よりも高い第３周波数ｆ３で加速度を検出した場合には、ｍ（＝ｆ３／ｆ２）個取得された加速度を加算平均すればよい。加速度は角速度と比較して、例えば位置を算出される場合などに積分される回数が少ないので、加算平均しても、積分による誤差の増大の影響が小さい。そのため、演算量を抑制し、かつ、以降の演算の精度低下を抑制できる。

（６）本発明の一態様では、前記姿勢更新部は、前記第２周波数ｆ２で前記姿勢変化算出部から前記姿勢変化を出力することを要求する要求信号を、前記姿勢変化算出部に出力することができる。

姿勢更新部から要求信号を出力することで、姿勢更新部へ姿勢変化を入力する周波数をより正確に保つことができるので、姿勢の算出精度低下を抑制できる。

（７）本発明の他の態様は、生体または前記生体のエネルギーを動力源として移動する移動体（以下、「移動体」と総称する。）の、慣性座標系に対する第１座標系の角速度を検出することと、第１周波数ｆ１で入力される前記角速度に基づいて、前記移動体の姿勢変化を算出することと、第２周波数ｆ２で入力される姿勢変化に基づいて、前記移動体の姿勢を更新することと、を含み、前記第１周波数及び前記第２周波数は、ｆ１＞ｆ２≧２０Ｈｚを満たす、姿勢算出方法に関する。

本発明方法においても、歩行運動する移動体のように角速度変化が比較的大きくても、積分精度の劣化を抑制でき、姿勢更新のための算出時にＣＰＵの負荷は低減される。

位置算出装置の全体システムの構成例を示す図である。 位置算出装置の機能ブロック図である。 ジャイロセンサーＩＣと処理部のブロック図である。 図３に示す機能ブロックの動作を示すフローチャートである。 ジャイロセンサーＩＣの詳細を示すブロック図である。 ジャイロセンサーＩＣ中の姿勢変化算出部の機能ブロック図である。 姿勢変化算出部のハードウェア構成図である。 図８（Ａ）はＣレジスタの初期化動作を、図８（Ｂ）はα行列演算を、図８（Ｃ）はｅｘｐ演算動作をそれぞれ説明するための図である。 姿勢変化算出部における姿勢変化の積算動作を示すフローチャートである。 比較例の動作を説明するためのフローチャートである。 ダッシュで移動する移動体の位置を本実施例と比較例とで算出した時に生ずる誤差を説明する図である。 ランニングで移動する移動体の位置を本実施例と比較例とで算出した時に生ずる誤差を説明する図である。 歩行で移動する移動体の位置を本実施例と比較例とで算出した時に生ずる誤差を説明する図である。 第２周波数ｆ２を低くした点を除いて図１１と同じ条件とした場合の比較結果を示す図である。 第２周波数ｆ２を低くした点を除いて図１２と同じ条件とした場合の比較結果を示す図である。 第２周波数ｆ２を低くした点を除いて図１３と同じ条件とした場合の比較結果を示す図である。 処理部の主要機能をハードウェアとした変形例の動作フローチャートを示す図である。

以下、図面を参照して、本発明の好適な実施形態の一例について説明する。但し、本発明を適用可能な実施形態が以下説明する実施形態に限定されるわけでないことは勿論である。

１．システム構成
図１は、本実施形態における位置算出装置１の全体システムの構成例を示す図である。この位置算出装置１は、生体または前記生体のエネルギーを動力源として移動する移動体、例えば人間の腰（右腰又は左腰）に装着して利用される小型電子機器であり、移動体の位置を算出して表示する位置算出装置の一種である。位置算出装置１は、移動体が位置の算出に係る各種操作を入力するための入力装置である操作ボタン３と、算出された位置等の情報が表示される液晶ディスプレイ５と、スピーカー７とを有する。なお、本明細書では、上記の生体および移動体を「移動体」と総称する。

本実施形態では、少なくとも３種類の座標系を定義する。第１の座標系は、位置算出装置１が具備するＩＭＵ１０（Inertial Measurement Unit）に対応付けられた三次元直交座標系（センサー座標系）であるローカル座標系である。本明細書では、ローカル座標系のことを、右手系で慣性センサーの検出軸を表したＢ（Body）フレームとも言う。本実施形態では、ローカル座標系の３軸をｘ軸、ｙ軸及びｚ軸と表記する。

第２の座標系は、移動体の移動空間を定める座標系である三次元直交座標系（絶対座標系）である。本明細書では、絶対座標系のことをＡ（Absolute）フレームとも言う。Ａフレームは、例えば、北東下座標系として知られるＮＥＤ（North East Down）座標系（Ｎフレーム）や、地球中心地球固定座標系として知られるＥＣＥＦ（Earth Centered Earth Fixed）座標系（Ｅフレーム）として定義される。本実施形態では、絶対座標系の３軸をＸ軸、Ｙ軸及びＺ軸と表記する。

第２の座標系は、移動体に対応付けられた三次元直交座標系（移動体座標系）であっても良い。本明細書では、移動体座標系のことをＶ（Vehicle）フレームとも言う。本実施形態では、移動体の前方を正とする前後方向をロール軸（Ｒ軸）、右方を正とする左右方向をピッチ軸（Ｐ軸）、鉛直下方を正とする上下方向をヨー軸（Ｑ軸）とする。

第３の座標系は、ＩＭＵ１０が検出を行う基準となる三次元直交座標系である慣性座標系である（図示せず）。本明細書では、Ｉ（Inertial）フレームとも言う。Ｉフレームは、地球の中心を原点とし、地球の自転軸を軸の１つとした座標系である。地球の自転は考慮しない。本実施形態では、慣性座標系の３軸をＬ軸、Ｍ軸及びＮ軸と表記する。本実施形態では、Ｎ軸が地球の自転軸であるものとする。

ＩＭＵ１０は、位置算出装置１に搭載されるセンサーユニットであり、慣性航法ユニットとして知られるものである。ＩＭＵ１０は、図２に示すように、加速度センサー１１と、ジャイロセンサー１３と、姿勢変化算出部を含むジャイロセンサーＩＣ１４とを有する。図１に示すようにＩＭＵ１０のｚ軸が鉛直方向の軸となる。

加速度センサー１１は、ローカル座標系の３軸（ｘ軸、ｙ軸及びｚ軸）について、対応する慣性座標系の各軸（Ｌ軸、Ｍ軸及びＮ軸）に対する加速度ベクトルを検出する。加速度センサー１１としては、例えばＭＥＭＳ（Micro Electro Mechanical Systems）センサーが用いられる。加速度センサー１１が計測した値は、ローカル座標系で計測した移動体の加速度となる。

本明細書では、原則として、特に断りなく加速度や速度と言ったときは方向及び大きさを考慮した加速度及び速度（すなわち、加速度ベクトルや速度ベクトル）を表すものとする。

ジャイロセンサー１３は、位置算出装置１が具備する加速度センサー１１と同じローカル座標系の３軸（ｘ軸、ｙ軸及びｚ軸）について、対応する慣性座標系の各軸（Ｌ軸、Ｍ軸及びＮ軸）に対する角速度（以下、「ローカル座標角速度」と称す。）を検出する。

図２に示すＧＰＳ(Global Positioning System）センサー１５は、測位用衛星の一種であるＧＰＳ衛星から送信されるＧＰＳ衛星信号を受信し、当該ＧＰＳ衛星信号を利用して移動体の位置及び速度ベクトルを算出するセンサーである。なお、ＧＰＳを利用して位置や速度ベクトルを算出する方法については従来公知であるため、詳細な説明を省略する。

位置算出装置１は、さらに、処理部１００と、図１に示す操作ボタン３を含む操作部２０と、図１に示す液晶ディスプレイ５を含む表示部３０と、図１に示すスピーカー７を含む音出力部４０と、通信部５０と、記憶部６０とを有して構成される。

処理部１００は、記憶部６０に記憶されているシステムプログラム等の各種プログラムに従って、位置算出装置１の各部を統括的に制御する。

２．ジャイロセンサーＩＣ及び処理部
図３は、ジャイロセンサーＩＣ１４及び処理部１００の主たる機能ブロックを示すブロック図である。図４は、図３に示す機能ブロックのシーケンス動作を示している。本実施形態においては、以下に説明する構成のうち、少なくとも姿勢変化算出部１１３および姿勢更新部１２４によって、姿勢算出装置の機能が実現される。姿勢算出装置は、さらに、ジャイロセンサー１３またはジャイロセンサーＩＣ１４、加速度センサー１１、座標変換部１２５のうち少なくとも１つを含んでいてもよい。

図３及び図４において、ジャイロセンサーＩＣ１４は、ジャイロセンサー１３が検出した角速度を例えば第１周波数ｆ１（例えばｆ１＝７．８１ｋＨｚ）でサンプリングしてＡ／Ｄ変換するＡ／Ｄ変換器１１１と、デジタルフィルター等のデジタル信号処理をするＤＳＰ(Digital Signal Processor)１１２と、ＤＳＰ１１２からの角速度に基づいて移動体の姿勢変化を算出する姿勢変化算出部１１３とを含む。姿勢変化算出部１１３は、角速度のバイアスを補正するジャイロバイアス補正部１１３Ａを含んでいる。姿勢変化算出部１１３は、第１周波数ｆ１でＤＳＰ１１２から入力される角速度に基づいて、移動体の姿勢変化を算出する。

図４に示すように、ジャイロセンサー１３により角速度が計測されるＳＴ１１）。Ａ／Ｄ変換器１１１及びＤＳＰ１１２を経由して角速度が入力される姿勢変化算出部１１３では、先ず、ジャイロバイアス補正部１１３Ａでジャイロバイアスが除去される（ＳＴ１２）。姿勢変化算出部１１３は、第１周波数ｆ１でＤＳＰ１１２から入力される角速度を、処理部１００からの姿勢出力要求に基づいて、第２周波数ｆ２（例えばｆ２＝２０Ｈｚ）で規定される期間（第２期間τ２）に亘って積分して、移動体の姿勢変化を算出する（ＳＴ１３）。姿勢変化算出部１１３は、算出された姿勢変化を出力した後に、姿勢変化をリセットする（ＳＴ１４）。

処理部１００は、加速度センサー１１が検出した例えば第３周波数ｆ３（例えばｆ３＝１００Ｈｚ）でサンプリングし加速度をＡ／Ｄ変換するＡ／Ｄ変換器１２１と、加速度のバイアスを補正する加速度バイアス補正部１２２と、第３周波数ｆ３によりサンプリングされた加速度をｍ（ｍは２以上の整数で、（ｆ３／ｆ２）−１＜ｍ≦ｆ３／ｆ２を満たす。例えばｍ＝５）個加算平均して、第２周波数ｆ２の出力周波数により加速度を出力する加算平均部１２３とを有する。図４に示すように、処理部１００では、加速度センサー１１からの加速度はＡ／Ｄ変換後にバイアスが除去され（ＳＴ２１）、５個の加速度が加算平均される（ＳＴ２２）。

処理部１００はさらに、図３に示すように、第２周波数ｆ２に従って姿勢変化算出部１１３から出力される姿勢変化に基づいて、移動体の姿勢を更新する姿勢更新部１２４を有する。姿勢変化算出部１１３は、第１周波数ｆ１でＤＳＰ１１２から入力される角速度に基づいて移動体の姿勢変化を算出する。姿勢変化算出部１１３は、姿勢更新部１２４からの要求に従い第２周波数ｆ２を出力周波数として姿勢変化を姿勢更新部１２４に出力する。図４に示すように、処理部１００は、第２周波数ｆ２に基づいて姿勢変化要求を姿勢変化算出部１１３に出力し（ＳＴ２３）、姿勢変化算出部１１３から得られた姿勢変化を用いて姿勢を更新する（ＳＴ２４）。

処理部１００はさらに、座標変換部１２５を有する。座標変換部１２５は、第２周波数ｆ２に従って姿勢更新部１２４で更新された移動体の姿勢と、第２周波数ｆ２で入力される加速度とに基づいて、第１座標系から第２座標系に座標変換された移動体の加速度（以下、「第２座標系加速度」と称する）を算出する。図４に示すように、処理部１００は、求められた姿勢から移動体の加速度の向きを更新する（ＳＴ２５）。

処理部１００はさらに、速度／位置算出部１２６を有する。図４に示すように、速度／位置算出部１２６は、座標変換部１２５から出力された第２座標系加速度を積分して、第２座標系における移動体の速度と位置とを算出する（ＳＴ２６，２７）。

以上の通り、位置算出装置１は、歩行運動する移動体の、第２座標系（絶対座標系、移動体座標系など）における位置を算出する。第１座標系から第２座標系に座標変換する際に、姿勢更新部１２４により更新された移動体の姿勢を用いる。移動体の姿勢（角度）は、ジャイロセンサー１３が検出した角速度を姿勢変化算出部１１３が積分することで得られた姿勢変化を用いて、前回の姿勢を更新することで得られる。積分する期間内の角速度のデータ量（サンプル数）が多いほど、つまり姿勢変化算出部１１３に入力される角速度の周波数（入力周波数）が高いほど、算出される姿勢の精度は高い。本実施形態では、姿勢変化算出部１１３に入力される角速度の周波数は第１周波数ｆ１であり、姿勢更新部１２４に入力される姿勢変化周波数（第２周波数ｆ２）よりも高い。よって、歩行運動する移動体のように角速度変化が比較的大きい場合にも、第２周波数ｆ２で積分を行う場合と比較して、積分精度の低下を抑制できる。一方、姿勢更新部１２４に入力される姿勢変化周波数は、第１周波数ｆ１よりも低い第２周波数ｆ２である。よって、姿勢更新以降の演算におけるＣＰＵの負荷は、姿勢更新部１２４に姿勢変化が第１周波数ｆ１で入力される場合と比較して低減される。

処理部１００はさらに、拡張カルマンフィルター演算部１２７及びエラー補正部１２８を有する。拡張カルマンフィルター演算部１２７は、図４に示すように、姿勢更新部１２４から出力される姿勢データと、速度／位置算出部１２６から出力された位置データ及び速度データと、ＧＰＳセンサー１５の測位データ（位置、速度、方位）と、加速度バイアス補正部１２２によって加速度バイアスが補正された加速度とを用いて、拡張カルマンフィルターによって、位置、速度、方位の誤差を推定する（ＳＴ２８）。また、拡張カルマンフィルター演算部１２７は、ジャイロバイアス及び加速度バイアスを推定し、加速度バイアス補正部１２２及びジャイロバイアス補正部１１３Ａに出力する。拡張カルマンフィルター演算部１２７によって推定された位置、速度、姿勢の誤差を用いて、移動体の位置、速度、姿勢の補正をエラー補正部１２８で行う。エラー補正部１２８からの情報が処理部１００の出力となる。

３．ジャイロセンサーＩＣの具体例
図５は、ジャイロセンサーＩＣ１４の具体例を示している。図５では、図３に示す構成に加えて、アナログ回路１１０、ＭＰＵ１１４及びシリアルパラレルインターフェース（ＳＰＩ）１１５が設けられている。

アナログ回路１１０は、ジャイロセンサー１３の出力をＱ−Ｖ変換し、増幅する。ＭＰＵ１１４は、ＳＰＩ１１５から入力されるコマンド／データに基づいて、アナログ回路１１０、Ａ／Ｄ変換器１１１、ＤＳＰ１１２、姿勢変化算出部１１３及びＳＰＩ１１５を制御する。そのために、ＭＰＵ１１４は、コマンドデコーダ１１４Ａとレジスタ１１４Ｂとを有する。コマンドデコーダ１１４Ａによりデコードされるコマンドに基づいて、レジスタに各種制御データが格納され、その制御データに基づいてジャイロセンサーＩＣ１４内の各所が制御される。ＳＰＩ１１５は、処理部１００に接続されるチップセレクト端子ＣＳ、データイン端子ＤＩ、データアウト端子ＤＯ、クロック端子ＣＬＫに加え、割り込み端子ＩＮＴを有する。ＭＰＵ１１４は、処理部１００の姿勢更新部１２４から、要求コマンドを、ＳＰＩ１１５を介して入力する。要求コマンドとは、第２周波数ｆ２を出力周波数として、姿勢変化算出部１１３から姿勢変化を出力要求するコマンドである。コマンドデコーダ１１４Ａによりデコードされた要求は、レジスタ１１４Ｂを介して姿勢変化算出部１１３に指令され、姿勢変化が出力される。割り込み端子ＩＮＴから第２周波数ｆ２の周期で割り込み出力を処理部１００へ出力してもよく、割り込み発生時に姿勢変化の出力を要求するコマンド（要求信号）を発行してもよい。

コマンドデコーダ１１４Ａにおいてデコードされるライト（Ｗ）コマンドによりレジスタ１１４Ｂに設定される制御値として、近似関数次数、バイアス更新タイミング、角速度変換（量子化単位LSB→rad/s）係数、DRDY出力タイミング、姿勢演算開始/停止、バイアス推定値書き込み等を挙げることができる。また、コマンドデコーダ１１４Ａによりデコードされるリード（Ｒ）コマンドに続いて、姿勢変化演算結果の読み出しを実施することができる。姿勢変化演算開始/停止タイミングは、第２周波数ｆ２の第２周期τ_２（τ_２＝１／ｆ２）に一致させることができる。

ここで、姿勢変化算出部１１３と姿勢更新部１２４と座標変換部１２５が行う演算の一例を次の式（１）に示す。

…（１）

本明細書では、座標変換行列を大文字の“Ｃ”で表す。各座標変換行列において、下付きの添え字は座標変換前の座標系を示し、上付きの添え字は座標変換後の座標系を示す。例えば、“Ｃ_ｂ ^ｅ”は、ローカル座標系（Ｂフレーム）から絶対座標系（Ｅフレーム））への座標変換行列を示す。

式（１）の左辺は、時刻ｔ＋τ_２における姿勢である。ｃ（ｔ）は、時刻ｔにおける姿勢の回転行列である。Ｃ_{ｃｈａｎｇｅ}は、時刻ｔから時刻ｔ＋τ_２の期間における姿勢変化である。式（１）の右辺のマイナス項は地球の自転の影響を表す。地球の自転の影響は省略しても良い。

式（１）のＣ_{ｃｈａｎｇｅ}は、次式（２）で示すことができる。

…（２）

式（２）において、τ１＝１／ｆ１であり、ｋは（ｆ１／ｆ２）−１＜ｋ≦ｆ１／ｆ２を満たす整数である。本明細書においては、自然対数の底ｅ＝２．７８１…のｄ乗を“ｅｘｐ（ｄ）”で表す（ｄは任意の数値、行列、数式等である）。また、本明細書においては、ｅｘｐ（[α（ｔ＋ｎτ_１）]×）を座標変換マトリックスと称する。式（２）は、第２周期τ２の期間に第１周期τ１の間隔で算出される総数ｋ個の座標変換マトリックスを掛け算することで、第２周期τ２毎の姿勢の変化を算出することを意味する。

式（２）のα（ｔ＋ｎτ_１）は角速度から求めた行列であり、次式（３）が成立する。ただし、ｎは１≦ｎ≦ｋが成立する整数である。

…（３）

式（３）の右辺は、時刻（t＋ｎτ_１）の時のＩフレームとＢフレームとの間の回転角度をＢフレームで表したものであり、ジャイロセンサー１３の検出値から得られる角速度である。よって、式（２）の右辺の行列（［α（ｔ＋ｎτ_１））］×）は角速度から求めることができる行列であり、次の式（４−１）（４−２）の通りとなる。

式（４−２）が示すように、式（４−１）の右辺の各要素は、時刻（t＋ｎτ_１）におけるジャイロセンサーの各軸の検出値をジャイロバイアスが補正した値に、第１周期τ_１を掛けた値である。

ここで、ｅｘｐ（α）の演算には、次の式（５）〜（７）に示す一次〜三次近似を用いることができる。

…（５）

…（６）

…（７）

図６は、姿勢変化算出部１１３の機能ブロック図である。なお、図６に示すレジスタ１３１〜１３４，１３６，１３９は、図５に示すレジスタ１１４Ｂの具体例である。図６に示す乗算器１３０には、ジャイロセンサー１３から入力された角速度のデータ（以下、「角速度データ」という）が入力される。乗算器１３０は、角速度データにレジスタ１３１の係数ｋｒを乗算することで、角速度をｒａｄ／ｓに変換する。レジスタ１３１に格納される係数は、図５に示すコマンドデコーダ１１４Ａのライトコマンドに基づいて格納することができる。レジスタ１３２〜１３４には式（４−２）のバイアス補正値（負値）が格納され、α行列演算部１３５内の加算器により角速度からバイアス補正値が減算される。当該加算器が、ジャイロバイアス補正部１１３Ａに相当する。バイアス補正値は、拡張カルマンフィルター演算部１２７からＳＰＩ１１５を介してレジスタ１３２〜１３４に所定のタイミングでセットされている。α行列演算部１３５は、式（４−１）の行列演算を実施する。

ｅｘｐ（α）演算部１３７では、式（５）〜（７）のいずれかの近似演算が、図５に示すコマンドデコーダ１１４Ａのライトコマンドに基づいてレジスタ（図示せず）にセットされた制御信号ＳｅｌＡｐｐで選択され、式（５）〜（７）のいずれかの近似演算が実施される。姿勢変化更新部１３８は、式（２）に従ってｅｘｐ（α）演算部１３７からの最新の姿勢変化データを過去の変化分に掛け合わせた結果をレジスタ１３９に格納する。レジスタ１３９に格納された姿勢変化データは、３×３の行列であり、式（２）のＣ_{ｃｈａｒｇｅ}に相当する。Ｃ_{ｃｈａｒｇｅ}は、姿勢更新部１２４からの要求（コマンド信号）に従って例えば４０ビット×９出力として、式（２）のＣ_{ｃｈａｒｇｅ}がＳＰＩ１１５を経由して姿勢更新部１２４に送出される。

４．ハードウェアで構成される姿勢変化算出部
図7は姿勢変化算出部１１３をハードウェアで構成した一例を示す回路図である。図７には、図６に示すα行列演算部１３５及びｅｘｐ（α）演算部１３７の機能をハードウェアとして実現する、レジスタ出力等を選択するセレクター１４１、１４２、１４４、１４５、１４７、１４８，レジスタ１４３、１５０〜１５５と、乗算器１４０と、加算器１４６とが設けられている。図７に示すレジスタ１３１〜１３４、１３６も、図５に示すレジスタ１１４Ｂの具体例である。

図６に示す姿勢変化算出部１１３の動作のうち、Ｃレジスタ１５３の初期化、α行列演算、及び式（５）に示す一次近似による演算動作が、図８（Ａ）〜図８（Ｃ）及び図９に示されている。図９のステップ１（ＳＴ１）でシステム（処理部１００）から姿勢変化の積算開始要求コマンドが入力されると、過去に計算して、Ｃレジスタ１５３に記憶（保持）されていた姿勢変化がリセット（消去）される（ＳＴ２）。具体的には、図８（Ａ）に示すように、図７に示すＣレジスタ１５３の初期化動作が開始される。乗算器１４０のＸ，Ｙ端子と、加算器１４６のＡ，Ｂ端子に、手順（図８のsequence、以下同様）１〜３において定数１または０をセットすることで、Ｃレジスタ１５３の領域ｃ１１，ｃ２２，ｃ３３が１にセットされ、他の領域に０がセットされる。

図９のステップ３において角速度が入力されたか否かが判断される。角速度が入力されると（ＳＴ３がＹＥＳ）、図９のＳＴ４，５が実施され、る。つまり、図８（Ｂ）に示すように、図６に示すα行列演算部１３５が、式（４）に示すα行列演算を図７に示すハードウェアを用いて実行する手順が示されている。手順１〜４で角速度データがバイアス補正され（図９のＳＴ４）、手順４〜７で式（４）の行列演算が実施され（図９のＳＴ５）、手順５〜８においてＡレジスタ１５０の９領域ａ１１〜ａ３３に３×３の行列が格納される。図８（Ｂ）では、Ａレジスタ１５０の領域ａ１１＝ａ２２＝ａ３３＝０であり、他の領域ａ１２，ａ１３，ａ２１，ａ２３，ａ３１，ａ３２には図８（Ｂ）に示すタイミングにおける乗算器１４０の出力ｏｐｏｕｔまたは加算器１４６の出力ｏａｄｄｅｒが格納される。

次に、図９のＳＴ６が実施される。図８（Ｃ）に示すように、図６に示すｅｘｐ（α）演算部１３７における式（５）に示す一次近似によるｅｘｐ（α）演算が、図７に示すハードウェアを用いて実施する手順が示されている。図８（Ｃ）では、Ａレジスタ１５０の領域ａ１１，ａ２２，ａ３３には図８（Ｃ）に示すタイミンクにおける加算器１４６の出力ｏａｄｄｅｒが格納される（図９のＳＴ７）。

図８（Ａ）〜図８（Ｃ）に示す姿勢変化の算出動作は、第１周波数ｆ１＝７．８１ｋＨｚで入力される角速度データに対して、第１周期τ_１＝１／ｆ１に従って繰り返し実施される。そのため、図９のＳＴ８に示すように、システム（処理部１００）から姿勢変化の出力要求があったか否かが判定される。図９のＳＴ８がＮＯである限り、図９のＳＴ３〜７が繰り返される。出力要求（コマンド信号）があったと判定されると、姿勢変化のデータは、Ｃレジスタ１５３を経由してＡｔｔレジスタ１３９を経て、処理部１００の姿勢更新部１２４に送出される（ＳＴ９）。なお、Ｔレジスタ１５１，Ｕレジスタ１５２，Ｔ１レジスタ１５４，Ｔ２レジスタ１５５等は、二次、三次近似計算の時に用いることができる。さらにレジスタを追加することで４次近似計算以上も対応することが可能である。また第１周波数ｆ１＝７．８１ｋＨｚは一例でありさらに高周波数を用いた演算も可能である。

図３の速度／位置算出部１２６は、式（２）により求められた今回の姿勢変化を用いて、式（１）によって今回の姿勢を算出する。そして、今回の姿勢と、過去に求めた姿勢と、加速度センサー１１が検出した加速度とに基づいて、下記の式（８）を演算して、Ｅフレームにおける加速度を求める。なお、ｆ_ｉｂ ^ｂ（ｔ＋τ_２）は時刻ｔ＋τ_２における加速度であり、時刻ｔ＋τ_２の時のＩフレームに対するＢフレームの加速度をＢフレームで表したものである。

…（８）

次に式（８）で求められた加速度と、過去に求めた速度とを用いて、下記の式（９）を演算して、Ｅフレームにおける速度を求める。ｖ_ｅｂ ^ｅ（ｔ）は、時刻ｔにおけるＥフレームに対するｂフレームの速度を、Ｅフレームで表したものである。なお、式（９）では、右辺の第３，４項において重力と地球の自転の影響が考慮されている。

…（９）

さらに、次に式（９）で求められた今回の速度と過去に求めた速度との平均値と、前回求めた位置とを用いて、下記の式（１０）を演算して、Ｅフレームにおける位置を求める。

…（１０）

５．比較例
図１０は、比較例の動作を説明するためのフローチャートである。図１０に示す比較例では、加速度センサー１１とジャイロセンサー１３からは、同一周波数（図１０では５００Ｈｚ）で加速度及び角速度が処理部１００に入力されている。

ここで、例えば、図１０に示す処理部１００では、角速度を積分して姿勢を演算する動作や位置の算出を含め、全て５００Ｈｚの周波数で実施した場合、処理部１００の処理負担は、実施形態のように第２周波数ｆ２＝２０Ｈｚで演算を行う場合と比較して重くなる。

処理部１００の負担を軽減する方法の比較例として、平均化について説明する。
次の式（１１）の通り複数データ分（複数周期分）の角速度を乗算せずに足し合わせる。これを平均化と称する。式（１１）に示されるように、第１周期τ１毎に１つの座標変換マトリックスを計算するのではなく、所定期間（τ３からｎτ３まで）の角速度を加算して、得られた行列を用いて座標変換マトリックスを計算する。本比較例ではｎ＝５周期分の角速度を平均化しているので、ｅｘｐ（α）の計算以降の演算はｆ＝１００Ｈｚで行われることになり、計算負荷を軽減することができる。なお、式（１１）において、τ_３は第３周期であり、τ_３＝１／ｆである。

…（１１）

式（１１）の平均化は、短時間における角速度の変化が比較的少ない車両等を移動体とする場合には誤差が少なく好適に用いることができた。ところが、本実施形態のように歩行運動する移動体は短時間における角速度の変化が比較的大きいため、平均化すると誤差が大きく、姿勢の算出精度が不十分であることが分かった。

図１１〜図１６は、本実施形態の実施例と比較例とで算出される移動体の位置の誤差を説明する図である。図１１〜図１６では、二次元位置座標（Ｘ，Ｙ）上で、スタート点（０，０）から図の矢印が示すように移動してエンド点（０，０）に戻るシミュレーションを行い、位置算出を行った。各図において、縦軸および横軸は位置座標を表す。単位はメートルである。破線は軌跡の真値を、一点鎖線は本実施形態の結果を、実線は比較例の結果を示している。図１１〜図１３に採用された本実施例では、第１周波数ｆ１＝７．８１ｋＨｚ、第２周波数ｆ２＝１００Ｈｚとされている。一方、図１１〜図１３に採用された比較例では、７．８１ｋＨｚの周波数で検出された角速度を平均化してｆ＝１００Ｈｚで姿勢演算を実施した。移動体としては人間を想定し、ダッシュ（全力で走行）、ランニング（駆け足）、歩行の3種類の方法で移動した場合について比較した。

図１１は移動体がダッシュで移動した例であり、図１２は移動体がランニングで移動した例であり、図１３は移動体が歩行により移動した例である。図１１〜図１３では、本実施例での算出結果は真値とほぼ一致し、誤差はほとんど生じていない。これに対して、比較例の算出結果はいずれも誤差が大きく、角速度変化が大きい図１１ではほとんど再現性がない算出結果となった。

図１４〜図１６に採用された本実施例では、第１周波数ｆ１＝７．８１ｋＨｚ、第２周波数ｆ２＝２０Ｈｚとされている。一方、図１４〜図１６に採用された比較例では、図１１〜図１３と同様に７．８１ｋＨｚの周波数で検出された角速度を平均化してｆ＝１００Ｈｚで姿勢演算を実施した。

図１４は移動体がダッシュで移動した例であり、図１５は移動体がランニングで移動した例であり、図１６は移動体が歩行により移動した例である。第２周波数ｆ２を低く設定した結果として、図１４〜図１６に示す実施例での算出結果は、図１１〜図１３に示すものと比較すると誤差は大きくなった。しかし、本実施例よりも高い周波数の１００Ｈｚで計算された比較例よりも誤差は少なくいことが分かる。図１６から分かるように、歩行程度の角速度の変化であれば、第２周波数ｆ２が２０Ｈｚと低い値であっても、移動体の移動を充分に再現できることが分かる。したがって、実施形態において、姿勢の更新をｆ１＞ｆ２≧２０Ｈｚで行うことで、演算の負荷を抑制し、かつ、移動体の姿勢を適切に算出することができる。

６．変形例
本発明を適用可能な実施形態は、上記の第１〜第３の実施形態に限定されることなく、本発明の趣旨を逸脱しない範囲で適宜変更可能であることは勿論である。以下、変形例について説明する。なお、上記実施形態と同一の構成については同一の符号を付して再度の説明を省略する。

６−１．システムの全体構成
図１７は、図３に示すジャイロセンサーＩＣの機能ブロックを処理部１００に取り込み、処理部の主要機能をハードウェアとした変形例での動作フローチャートを示している。ここで、図１７に示すＳＴ４１〜ＳＴ４３は図４のＳＴ１２〜ＳＴ１４と同じである。また、図１７に示すＳＴ５１〜ＳＴ５７は図４に示すＳＴ２１、ＳＴ２３〜ＳＴ２８と同じである（ＳＴ２２に相当する動作が省略されている）。演算の内容自体は図４の実施形態と同様であるので説明を省略し、図４の実施形態と異なる部分を説明する。図１７では、第１周波数ｆ１＝１０ｋＨｚとし、第２周波数ｆ２＝１ｋＨｚ、第３周波数ｆ３＝１ｋＨｚとして、図３に示す加算平均部１２３を不要とした。この場合でも、上述した実施形態と同様に姿勢変化算出部１１３はハードウェア化されているので、ＣＰＵに負担をかけずに重い演算をハードウェアにより実施できる。

加えて、姿勢更新部１２４もハードウェア化されているので、上述の実施形態の第２周波数ｆ２＝２０Ｈｚよりも格段に高いｆ２＝１ｋＨｚとすることができ、姿勢更新も精度高く実施することができる。

６−２．姿勢算出装置
上記の実施形態では、位置算出装置１が姿勢算出装置の機能を含む例を説明したが、姿勢算出装置を独立の装置として構成してもよい。例えば、ジャイロセンサー１３と、ジャイロセンサーＩＣ１４と、姿勢更新部１２４とを含む機器とすることが可能である。

６−３．姿勢変化算出部
上記の実施形態では、姿勢変化算出部１１３がハードウェアである例を中心に説明したが、姿勢変化算出部１１３はソフトウェアで構成することも可能である。この場合、姿勢変化算出部１１３は処理部１００の一部として構成してもよい。

６−４．ＧＰＳセンサー
上記の実施形態では、位置算出装置１がＧＰＳセンサー１５を備える例を説明したが、ＧＰＳセンサーは省略してもよい。

６−５．移動体
上記の実施形態では、移動体として人間を例として説明したが、移動体は人間に限定されない。平均化を適用して算出した姿勢または位置の誤差が許容できない場合には、本発明を適用することで効果を得ることができる。例えば、生体は人間以外の動物であってもよい。移動体の動作は歩行やランニングに限定されず、跳躍、遊泳、滑走、または飛行等であってもよい。生体のエネルギーを動力源として移動する移動体の例としては、スキー板、スケート靴、自転車、ハンググライダー、ボート、橇等が例示できる。ＩＭＵ１０は、生体に装着しても、生体のエネルギーを動力源として移動する移動体に装着してもよい。

本発明の新規事項および効果から実体的に逸脱しない多くの変形が可能であることは当業者には容易に理解できるであろう。従って、このような変形例はすべて本発明の範囲に含まれるものとする。例えば、明細書又は図面において、少なくとも一度、より広義又は同義な異なる用語と共に記載された用語は、明細書又は図面のいかなる箇所においても、その異なる用語に置き換えることができる。なお、第１，第２座標系は三次元に限らず二次元であっても良い。

１ 位置算出装置、１０ センサーユニット、１１ 加速度センサー、１３ ジャイロセンサー、１４ ジャイロセンサーＩＣ、１１３ 姿勢変化算出部、１２２ 加算平均部、１２４ 姿勢更新部、１２５ 座標変換部、１２６ 速度／位置算出部、１３０ 乗算器、１４６ 加算器

Claims (7)

1. 生体または前記生体のエネルギーを動力源として移動する移動体（以下、「移動体」と総称する。）の、慣性座標系に対する第１座標系の角速度を検出するジャイロセンサーと、
   第１周波数ｆ１で入力される前記角速度に基づいて、前記移動体の姿勢変化を算出する姿勢変化算出部と、
   第２周波数ｆ２で入力される前記姿勢変化に基づいて、前記移動体の姿勢を更新する姿勢更新部と、
   を含み、
   前記第１周波数及び前記第２周波数は、ｆ１＞ｆ２≧２０Ｈｚを満たす、
   姿勢算出装置。
2. 請求項１記載の姿勢算出装置において、
   前記姿勢変化算出部は、
   ｅｘｐ（［α_ｉｂ ^ｂ（ｔ＋τ_１）］×）ｅｘｐ（［α_ｉｂ ^ｂ（ｔ＋２τ_１）］×）・・・ｅｘｐ（［α_ｉｂ ^ｂ（ｔ＋ｋτ_１）］×）
   を演算する、姿勢算出装置。
   ただし、τ_１＝１／ｆ１であり、ｋは（ｆ１／ｆ２）−１＜ｋ≦ｆ１／ｆ２を満たす整数であり、ｔは時刻であり、［α_ｉｂ ^ｂ（ｔ＋ｎτ_１）］は時刻ｔ＋ｎτ_１（ただしｎは１≦ｎ≦ｋを満たす整数）における前記角速度から求めた回転行列である。
3. 請求項２に記載の姿勢算出装置において、
   前記慣性座標系に対する前記１座標系の加速度を検出する加速度センサーと、
   前記姿勢と、前記第２周波数ｆ２で入力される前記加速度とに基づいて、前記第１座標系から第２座標系に前記加速度を座標変換する座標変換部と、
   をさらに含む、姿勢算出装置。
4. 請求項１〜３のいずれか１項記載の姿勢算出装置において、
   前記ジャイロセンサーが収容されるセンサーユニットをさらに含み、
   前記姿勢変化算出部は、前記センサーユニットに収容されるハードウェアによって構成される、姿勢算出装置。
5. 請求項１〜４のいずれか１項記載の姿勢算出装置において、
   第３周波数ｆ３（ｆ２＜ｆ３）で入力された前記加速度を、ｍ（ｍは２以上の整数で、（ｆ３／ｆ２）−１＜ｍ≦ｆ３／ｆ２））個加算平均して、前記第２周波数ｆ２で前記座標変換部に入力させる加算平均部をさらに含む、姿勢算出装置。
6. 請求項１〜５のいずれか１項記載の姿勢算出装置において、
   前記姿勢更新部は、前記姿勢変化を出力することを要求する要求信号を前記姿勢変化算出部に出力する、姿勢算出装置。
7. 生体または前記生体のエネルギーを動力源として移動する移動体（以下、「移動体」と総称する。）の、慣性座標系に対する第１座標系の角速度を検出することと、
   第１周波数ｆ１で入力される前記角速度に基づいて、前記移動体の姿勢変化を算出することと、
   第２周波数ｆ２で入力される姿勢変化に基づいて、前記移動体の姿勢を更新することと、
   を含み、
   前記第１周波数及び前記第２周波数は、ｆ１＞ｆ２≧２０Ｈｚを満たす、
   姿勢算出方法。

Images