JP6167747B2 - バイアス算出方法及びバイアス算出装置 - Google Patents

Description

本発明は、加速度センサーのバイアスを算出するバイアス算出方法及びバイアス算出装置に関する。

現在、いわゆるシームレス測位やモーションセンシング、姿勢制御など様々な分野において、慣性センサーの活用が注目されている。慣性センサーとしては、加速度センサーやジャイロセンサー、圧力センサー、地磁気センサーなどが広く知られている。

例えば慣性センサーの一種である加速度センサーの出力には、ゼロ点バイアスに代表されるバイアス（加速度バイアス）が含まれる。加速度センサーの出力は、例えば慣性航法演算などに用いられるが、加速度バイアスによってその演算結果に大きな誤差が生じる可能性がある。このため、予め加速度バイアスを算出（推定）しておき、加速度センサーの出力値を用いる際に、この加速度バイアスを考慮する必要がある。加速度バイアスを算出する技術として、特許文献１には、加速度センサーの検出軸を鉛直方向（重力が作用する方向）に一致させて配置することで、当該検出軸の加速度バイアスを算出できることが開示されている。

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

しかしながら、上述の従来の技術は、一軸分の加速度バイアスを算出するものである。つまり、複数軸（多軸）の加速度センサーの場合、これらの複数の検出軸（３軸）それぞれについて、順に１軸ずつ、鉛直方向に一致させて配置して当該検出軸の加速度バイアスの算出を行う必要があり、手間がかかる。

ところで、加速度センサーは、外界の温度変化による影響を受けやすいため、温度補償が行われている。しかし、温度補償を行っても、電源を入れる度に生じるバイアスであるターンオンバイアスや、温度以外の外界の影響により、加速度センサーの出力に含まれるバイアスはゼロとならない。このため、電源を投入して使用を開始する毎に、加速度バイアスの算出（初期キャリブレーション）を行う必要があるが、その度に、上述のように、各軸それぞれについて、鉛直方向に向けて加速度バイアスの算出を行うことは、非常に面倒であった。

本発明は上述した課題に鑑みてなされたものであり、その目的とするところは、加速度センサーのバイアス値の算出を、手間を軽減して容易に行えるようにすることである。

上記課題を解決するための第１の発明は、加速度センサーとジャイロセンサーとを有するセンサー部を、前記加速度センサーの第１軸及び第２軸がなす平面を鉛直方向に対して略垂直にした状態で、前記鉛直方向を軸中心に回転させることと、前記センサー部の検出値を用いて前記加速度センサーの前記第１軸及び前記第２軸のバイアスを算出することと、を含む加速度バイアス算出方法である。

また、他の発明として、
加速度センサーとジャイロセンサーとを有するセンサー部の前記加速度センサーのバイアスを算出するバイアス算出装置であって、前記加速度センサーの第１軸及び第２軸がなす平面を鉛直方向に対して略垂直にした状態で、前記鉛直方向を軸中心にして前記センサー部を回転させる旨の報知を行う報知制御部と、前記センサー部の検出値を用いて前記加速度センサーの前記第１軸及び前記第２軸のバイアスを算出するバイアス算出部と、を備えたバイアス算出装置を構成しても良い。

この第１の発明等によれば、加速度センサーとジャイロセンサーとを有するセンサー部を、加速度センサーの第１軸及び第２軸がなす平面を略水平にした状態で鉛直方向を軸中心にして回転させることで、加速度センサーの第１軸及び第２軸の両軸のバイアスを算出することができる。

また、第２の発明は、第１の発明の加速度バイアス算出方法であって、前記バイアスを算出することは、前記バイアスを未知数として前記センサー部の姿勢を前記検出値を用いて表した瞬時成立条件を、前記回転中に複数算出することと、複数の前記瞬時成立条件を用いて前記バイアスを算定することと、を含む加速度バイアス算出方法である。

この第２の発明によれば、バイアスを未知数としてセンサー部の姿勢を検出値を用いて表した瞬時成立条件を回転中に複数算出し、算出した瞬時成立条件を用いて、加速度センサーのバイアスが算出される。回転中は、センサー部の姿勢は時々刻々と変化する。つまり、センサー部の姿勢が異なる複数の状態それぞれにおける瞬時成立条件が生成されることになる。瞬時成立条件の数が多いほど、バイアスの算出精度を高めることができる。

また、第３の発明は、第２の発明の加速度バイアス算出方法であって、前記センサー部の検出値を用いて前記センサー部の姿勢角を判定することを更に含み、前記瞬時成立条件を算出することは、前記検出値と、前記姿勢角と、重力加速度とを用いて前記瞬時成立条件を算出することである、加速度バイアス算出方法である。

この第３の発明によれば、瞬時成立条件は、センサー部の検出値と、センサー部の検出値を用いて判定されるセンサー部の姿勢角と、重力加速度とを用いて算出される。

また、第４の発明は、第３の発明の加速度バイアス算出方法であって、前記姿勢角を判定することは、前記センサー部を静止させた状態での前記加速度センサーの検出値と、重力加速度とを用いて初期の姿勢角を判定することと、従前に判定した姿勢角を前記ジャイロセンサーの検出値を用いて更新することで複数の前記姿勢における姿勢角を判定することとを含む、加速度バイアス算出方法である。

この第４の発明によれば、姿勢角の判定として、センサー部を静止させた状態での加速度センサーの検出値と重力加速度とを用いて初期の姿勢角を判定し、従前に判定した姿勢角をジャイロセンサーの検出値を用いて更新することで、複数の姿勢における姿勢角の判定が行われる。

また、第５の発明は、第２〜第４の何れかの発明の加速度バイアス算出方法であって、前記バイアスを算定することは、複数の前記瞬時成立条件を用いた所定の近似計算を行って前記バイアスを算定することである、加速度バイアス算出方法である。

この第５の発明によれば、複数の瞬時成立条件を用いた所定の近似計算を行うことで、バイアスが算出される。

また、第６の発明は、第１〜第５の何れかの発明の加速度バイアス算出方法であって、前記回転させることは、前記第１軸と前記鉛直方向に対して垂直な水平面とがなす角度と、前記第２軸と前記水平面とがなす角度との和が１０度以下の状態で、前記センサー部を回転させることである、加速度バイアス算出方法である。

この第６の発明によれば、第１軸と前記鉛直方向に対して垂直な水平面とがなす角度と、第２軸と水前記平面とがなす角度との和が１０度以下の状態で、センサー部を回転することで、加速度バイアスを算出できる。

また、第７の発明は、第１〜第６の何れかの発明の加速度バイアス算出方法であって、前記センサー部は、ユーザーの胴体部、頭部、或いは脚部に装着されて用いられる、加速度バイアス算出方法である。

この第７の発明によれば、加速度センサーの第１軸及び第２軸がなす平面が略水平となる状態で、センサー部を、ユーザーの胴体部、頭部、或いは脚部に装着し、ユーザーがその場で左右方向に回転するといった簡易な動作で、加速度センサーのバイアスを算出することができる。

位置算出装置の全体構成図。 加速度バイアスの算出実験結果。 位置算出装置の機能構成図。 加速度バイアス算出用データのデータ構成例。 加速度バイアスデータのデータ構成例。 加速度バイアス算出処理のフローチャート。

［全体構成］
図１は、本実施形態における位置算出装置１の構成図である。この位置算出装置１は、ユーザー２０の腰部に装着して利用される小型の電子機器であり、慣性航法演算を行うことで当該装置の位置を算出することができる。位置算出装置１には、操作スイッチ３やディスプレイ５、スピーカー７等が設けられているとともに、ＩＭＵ（Inertial Measurement Unit）１０と、ＣＰＵ（Central Processing Unit）やメモリーを実装した制御装置とを内蔵している。

ＩＭＵ１０は、加速度センサー１２と、ジャイロセンサー１４とを有するセンサーユニットである。加速度センサー１２は、当該センサーに対応付けられた三次元直交座標系であるローカル座標系（センサー座標系）における加速度を検出する。ジャイロセンサー１４は、当該センサーに対応付けられた三次元直交座標系であるローカル座標系（センサー座標系）における角速度を検出する。なお、加速度センサー１２のセンサー座標系、及び、ジャイロセンサー１４のセンサー座標系は座標軸が同一として説明するが、異なる場合には、座標変換行列演算を行えば、一方の座標系を他方の座標系に置換することが可能である。座標変換行列演算については公知の手法を採用可能であるため説明を省略する。

そして、この位置算出装置１は、ローカル座標系のＺ軸が鉛直下向きとなり、Ｘ−Ｙ平面が略水平（鉛直方向に対して略垂直）となる姿勢を推奨姿勢として、ユーザー２０に装着されるように構成されている。本実施形態においては、Ｘ軸及びＹ軸が、第１軸及び第２軸に相当する。また、この位置算出装置１は、加速度センサー１２の各検出軸のバイアスを算出するバイアス算出装置でもある。

［原理］
本実施形態において、加速度センサー１２の出力は温度補償されていることとする。すなわち、加速度センサー１２の出力には、ゼロ点バイアスやスケールファクター、二次感度といった温度の依存する成分（温度依存成分）が含まれる。これらの温度依存成分の温度特性が加速度センサー１２の製造段階において求められ、補償される。なお、温度補償は、加速度センサー１２の内部で行っても良いし、外部で行っても良い。すなわち、加速度センサー１２内部に温度補償用の回路部を設けて、加速度センサー１２の出力値が既に温度補償されているものとしても良いし、加速度センサー１２とは別の処理回路部が加速度センサー１２の出力値を温度補償（より正確には補正）することとしても良い。いずれにせよ、本実施形態においては、加速度センサー１２で検出され、温度補償された後の値を、加速度センサー１２の検出値として説明する。

しかしながら、温度補償を行っても、電源投入の度に生じるターンオンバイアスや、温度依存成分以外のバイアス成分により、検出値に含まれるバイアスが完全にゼロとならず、バイアス値が残存する。この検出値に残存するバイアスの大きさを推定することが、本実施形態の目的である。

（Ａ）静止時
先ず、静止時における加速度センサー１２のバイアスの推定について説明する。静止時には、加速度センサー１２には重力のみが作用する。また、推定する加速度センサー１２の各検出軸（Ｘ，Ｙ，Ｚ軸）のバイアス値を、ｂ_ａ＝（ｂ_ａｘ，ｂ_ａｙ，ｂ_ａｚ）、とする。

理想状態として、加速度センサー１２を、Ｚ軸を鉛直下向きに設置した場合（すなわち、Ｘ−Ｙ平面が完全に水平となるように設置した場合）、加速度センサー１２の各検出軸（Ｘ，Ｙ，Ｚ軸）の検出値ｆ＝（ｆ_ｘ，ｆ_ｙ，ｆ_ｚ）は、次式（１）で与えられる。つまり、加速度センサー１２の各検出軸（Ｘ，Ｙ，Ｚ軸）のバイアス値ｂ_ａ＝（ｂ_ａｘ，ｂ_ａｙ，ｂ_ａｚ）を容易に求められる。

しかし、実際には、Ｘ−Ｙ平面が完全に水平となるように加速度センサー１２を設置することは困難である。水平面（鉛直方向に対して垂直な平面）に対するＸ−Ｙ平面の傾きを加速度センサー１２の姿勢とし、Ｘ軸周りの回転角であるピッチ角φ、及び、Ｙ軸周りの回転角であるロール角θによって表現すると、加速度センサー１２の各検出軸（Ｘ，Ｙ，Ｚ軸）の検出値ｆ＝（ｆ_ｘ，ｆ_ｙ，ｆ_ｚ）は、次式（２）で与えられる。

この式（２）は行列表現であるが、行列を用いない表現に改めると、次式（３）のようになる。

式（３）からわかるように、加速度センサー１２の検出値ｆ＝（ｆ_ｘ，ｆ_ｙ，ｆ_ｚ）には、重力加速度ｇの各軸方向成分と、バイアス成分との２種類が含まれる。加速度センサー１２の姿勢角φ，θが不明であるため、この式（３）からは、バイアス値ｂ_ａ＝（ｂ_ａｘ，ｂ_ａｙ，ｂ_ａｚ）を算出することは困難である。そこで、本実施形態では、式（３）において、バイアスをゼロと仮定した場合の姿勢角である仮姿勢角を用いて、加速度センサー１２の検出値に含まれるバイアス値を推定する。

式（３）において、加速度センサー１２のバイアスをゼロ（ｂ_ａｘ＝０，ｂ_ａｙ＝０，ｂ_ａｚ＝０）と仮定した場合の姿勢角である仮の姿勢角φ^ｔ、θ^ｔは、次式（４）となる。

加速度センサー１２はＸ−Ｙ平面が略水平に配置されているため、姿勢角φ，θは十分に小さい（φ＋θ≦１０度）。このため、仮の姿勢角φ^ｔ、θ^ｔは、近似的に次式（５）によっても求められる。

また、この仮の姿勢角φ^ｔ，θ^ｔと実際の姿勢角φ，θとの差である姿勢角誤差δφ，δθは、次式（６）となる。

そして、式（３）において、仮の姿勢角φ^ｔ，θ^ｔから算出（逆算）されるバイアス値を、仮のバイアス値ｂ^ｔ＝（ｂ_ａｘ ^ｔ，ｂ_ａｙ ^ｔ，ｂ_ａｚ ^ｔ）、とする。また、この仮のバイアス値ｂ^ｔと実際のバイアス値ｂ^ｔの差をバイアス誤差δｂとする。

さて、本実施形態のポイントの１つとして、加速度センサー１２のゼロ点バイアスのばらつきが誤差分散σｍｇ以下ならば、ゼロ点バイアスを無視して、静止時の加速度センサー１２の検出値から得られる姿勢角φ，θの誤差は、ほぼarcsin（０．００１σ）以下となる発見がある。

バイアス値の誤差分散σは、加速度センサー１２のバイアス値の誤差の広がりを表す。本実施形態では、加速度センサー１２の出力は温度補償されているとしているため、検出値に含まれるバイアス値は温度依存成分による誤差が低減された値となる。しかし、製品間のばらつきにより、全ての加速度センサー１２においてバイアス値が均一とはならない。

製品間のばらつきは、加速度センサー１２の製造段階において、例えば試験装置を用いた試験を行うことで測定することができる。つまり、加速度センサー１２の製品それぞれについて温度補償を行った後、加速度センサー１２の検出値に残存するバイアス値を測定することで、バイアス値の誤差分布が求まる。そして、このバイアス値の誤差分布から、バイアス値の誤差の広がりを誤差分散σとして求める。誤差分散σは、例えばバイアス値の標準偏差や分散値として求めることができる。

近年では、小型で安価な慣性センサーとして、半導体の微細加工技術を応用したＭＥＭＳセンサーが種々の民生機器に搭載されている。このＭＥＭＳセンサーを例に挙げると、温度補償された加速度センサー１２のバイアス値は、ｍｇ＝０．００１ｇ、のオーダーの値となる。本実施形態では、ＭＥＭＳセンサーによる加速度センサー１２を想定して、バイアス値の誤差分散σを、「σｍｇ」とする。

先ず、ロール角θについて説明する。式（３）におけるＸ軸の検出値ｆ_ｘの式から、次式（８）が得られる。

加速度センサー１２のゼロ点バイアスのばらつきが誤差分散σｍｇ以下（ｂ_ａｘ≦σｍｇ）であることから、式（８）は、次式（９）となる。

式（５），（６）から、式（９）は次式（１０）となる。

ロール角θ、及び、ロール角誤差δθは十分小さい（φ＋θ≦１０度）ため、ｃｏｓθ≒１、ｃｏｓδθ≒１、と近似すると、式（１０）は、次式（１１）となる。

このロール角誤差δθを表す式（１１）によれば、加速度センサー１２のゼロ点バイアスの誤差が１０ｍｇ以下ならば、このゼロ点バイアスを無視して求められるロール角角誤差δθ、及び、ピッチ角誤差δφは、ほぼ０．６ｄｅｇ以下となる。

続いて、式（３）において、加速度センサー１２のＺ軸のバイアス値ｂ_ａｚは、次式（１２）である。

この式（１２）から、仮の姿勢角φ^ｔ，θ^ｔを用いて、Ｚ軸の仮のバイアス値ｂ_ａｚを表す次式（１３）が成立する。

この式（１３）は、式（６）から、次式（１４）となる。

姿勢角誤差δφ，δθは十分小さい（φ＋θ≦１０度）ため、ｃｏｓθ≒１、ｃｏｓδθ≒１、δθ≒δφ、ｓｉｎδφ・ｓｉｎδθ≒０、と近似すると、式（１４）は、次式（１５）となる。

式（７）から、加速度センサー１２のＺ軸のバイアス値ｂ_ａｚの誤差δｂ_ａｚは、次式（１６）となる。

更に、式（１１）から、式（１６）は、次式（１７）となる。

また、加速度センサー１２のＸ軸のバイアス値ｂ_ａｘの誤差δｂ_ａｘについても同様に求められ、次式（１８）となる。

また、加速度センサー１２のＹ軸のバイアス値ｂ_ａｙの誤差δｂ_ａｙについても同様に求められ、次式（１９）となる。

このように、式（１７）〜（１９）から、静止時における加速度センサー１２の各検出軸（Ｘ，Ｙ，Ｚ軸）のバイアス誤差δｂ_ａｘ，δｂ_ａｙ，δｂ_ａｚは、姿勢角φ，θ、及び、姿勢角誤差δφ，δθに依存することがいえる。

また、加速度センサー１２は、Ｘ−Ｙ平面が水平になるように配置されるため、姿勢角φ，θは小さい。この姿勢角φ，θが小さいほど、ｓｉｎ（θ＋φ）→０、ｃｏｓθ，ｃｏｓ（θ＋φ）→１、となるため、Ｚ軸のバイアス誤差δｂ_ａｚのほうが、Ｘ，Ｙ軸の誤差δｂ_ａｘ，δｂ_ａｚよりも小さいといえる。例えば、姿勢角φ，θの和が１０度以下（φ＋θ≦１０度）であり、加速度センサー１２のゼロ点バイアスがσｍｇの場合、各検出軸（Ｘ，Ｙ，Ｚ軸）の推定されるバイアス誤差δｂ_ａｘ，δｂ_ａｙ，δｂ_ａｚは、次式（２０）となる。

式（２０）によれば、静止時には、Ｚ軸のバイアス誤差δｂ_ａｚが小さいため、式（３）からＺ軸のバイアス値ｂ_ａｚを算出可能である。一方、Ｘ，Ｙ軸については、式（１８），（１９）によれば、バイアス誤差δｂ_ａｘ，δｂ_ａｙが大きい（誤差分散σに近い）ため、バイアス値ｂ_ａｘ，ｂ_ａｙを正確に算出し難い。

（Ｂ）回転
そこで、本実施形態は、加速度センサー１２を、Ｚ軸を回転軸として軸回転（鉛直方向を軸中心に回転させる）させることで、Ｘ，Ｙ軸のバイアス値ｂ_ａｘ，ｂ_ａｙを算出する。上述のように、静止時には、鉛直下向きの重力のみが作用する。加速度センサー１２を回転させた場合、Ｚ軸と回転軸とがほぼ一致する場合、重力以外の回転力を無視できるため、静止時と同様に、鉛直下向きの重力のみが作用しているとみなすことができる。具体的には、図１に示すように、ユーザー２０は、Ｚ軸を鉛直下向きに略一致させて位置算出装置１を装着した状態で、矢印２２が示すように、腰を左右方向（水平方向）に回転させる動作を行う。

先ず、加速度センサー１２の静止時を時刻ｔ_０とし、このときの初期姿勢角φ_０，θ_０から、Ｘ軸の検出値ｆ_ｘ０は、次式（２１）となる。

また、加速度センサー１２の回転時には、姿勢角φ，θが変化する。回転中の時刻ｔ_ｉ（ｉ＝１，２，・・・）における姿勢角φ_ｉ，θ_ｉから、時刻ｔ_ｉにおけるＸ軸の検出値ｆ_ｘは次式（２２）となる。但し、時刻ｔ_ｉは、ｔ_ｉ＝ｔ_ｉ−１＋ｄｔ、と定められることとする。

また、姿勢角φ，θは、ジャイロセンサー１４の検出値（角速度ω）によって求められる。すなわち、時刻ｔ_ｉにおけるジャイロセンサー１４の各検出軸（Ｘ，Ｙ，Ｚ軸）の検出値（角速度）を、ω_ｉ＝（ω_ｘｉ、ω_ｙｉ，ω_ｚｉ）、とすると、時刻ｔ_１における仮の姿勢角φ_１ ^ｔ，θ_１ ^ｔは、直前の時刻ｔ_０における仮の姿勢角φ_０ ^ｔ，θ_０ ^ｔに対する更新値として与えられ、次式（２３）となる。

従って、式（２２）から、時刻ｔ_１におけるバイアス値ｂ_ａｘを表す次式（２４）が成り立つ。

また同様に、次の時刻ｔ_２における仮の姿勢角φ_２ ^ｔ，θ_２ ^ｔは、直前の時刻ｔ_１における仮の姿勢角φ_１ ^ｔ，θ_１ ^ｔに対する更新値として与えられ、次式（２５）となる。

従って、式（２２）から、時刻ｔ_２におけるバイアス値ｂ_ａｘを表す次式（２６）が成り立つ。

更に、次の時刻ｔ_３についても同様に、仮の姿勢角φ_３ ^ｔ，θ_３ ^ｔは、直前の時刻ｔ_２における仮の姿勢角φ_２ ^ｔ，θ_２ ^ｔに対する更新値として与えられ、この仮の姿勢角φ_３ ^ｔ，θ_３ ^ｔを用いて、式（２２）から、時刻ｔ_３におけるバイアス値ｂ_ａｘについての式「ｆ_ｘ３−ｇ・ｓｉｎθ_３ ^ｔ」を求めることができる。以降の時刻ｔ_３，ｔ_４，・・についても同様である。

このように、式（２４），（２６）によれば、Ｘ軸のバイアス値ｂ_ａｘを表す式「ｆ_ｘ−ｇ・ｓｉｎθ^ｔ」は、次式（２７）に示すように、未知数のバイアス値ｂ_ａｘ，ｂ_ａｙの線形結合になるといえる。この式（２７）を、「Ｘ軸の成立条件式」と呼ぶ。

この式（２７）において、係数Ａ，Ｂは、該当する時刻ｔ_ｉにおけるジャイロセンサー１４の検出値ω_ｚｉと、直前の時刻ｔ_ｉ−１におけるジャイロセンサー１４の検出値ω_{ｚ，ｉ−１}とによって決まる。つまり、加速度センサー１２を回転させて各時刻ｔ_ｉにおける加速度センサー１２の検出値ｆ_ｘｉを取得することで、各時刻ｔ_ｉにおけるＸ軸の成立条件式（式（２７））を得ることができる。そして、これらの各時刻ｔ_ｉ（ｉ＝０，１，・・・）におけるＸ軸の成立条件式を連立方程式とし、例えば最小二乗法といった近似計算によって、加速度センサー１２のバイアス値ｂ_ａｘ，ｂ_ａｙの近似解を求めることができる。

また、Ｙ軸についても同様である。すなわち、加速度センサー１２の静止時の時刻ｔ_０におけるＹ軸の検出値ｆ_ｙ０、及び、回転中の時刻ｔ_ｉ（ｉ＝１，２，・・・）におけるＹ軸の検出値ｆ_ｘは、次式（２８）となる。

この式（２８）から、時刻ｔ_１，ｔ_２それぞれにおけるバイアス値ｂ_ａｙを表す次式（２９），（３０）が、それぞれ成り立つ。

つまり、Ｙ軸のバイアス値ｂ_ａｙを表す式「ｆｙ＋ｇ・ｓｉｎφ^ｔ・ｃｏｓθ^ｔ」は、次式（３１）に示すように、未知数のバイアス値ｂ_ａｘ，ｂ_ａｙの線形結合となるといえる。この式（３１）を、「Ｙ軸の成立条件式」と呼ぶ。

この式（３１）において、係数Ｃ，Ｄは、該当する時刻ｔ_ｉにおけるジャイロセンサー１４の検出値ω_ｚｉと、直前の時刻ｔ_ｉ−１におけるジャイロセンサー１４の検出値ω_{ｚ，ｉ−１}とによって決まる。つまり、加速度センサー１２を回転させて各時刻ｔ_ｉにおける加速度センサー１２の検出値ｆ_ｙｉを取得することで、各時刻ｔ_ｉにおけるＹ軸の成立条件式（式（３１））を得ることができる。そして、これらの各時刻ｔ_ｉ（ｉ＝０，１，・・・）におけるＹ軸の成立条件式を連立方程式とし、例えば最小二乗法といった近似計算によって、加速度センサー１２のバイアス値ｂ_ａｘ，ｂ_ａｙの近似解を求めることができる。

［実験結果］
本実施形態の加速度バイアスｂ_ａの算出原理を用いた実験結果を説明する。図２は、被験者が、ＩＭＵ（を含む装置）を腰部に装着して腰回転動作を行った場合の実験結果である。本実験の１回の腰回転動作は、腰を一方向（例えば右方向）にひねり、次いで反対方向（例えば左方向）にひねり、正面を向くところまでを含んでいる。正面を向いた後はそのまま静止した。図２では、横軸を時刻ｔ、縦軸を加速度センサー１２のＸ，Ｙ軸のバイアス値ｂ_ａｘ，ｂ_ａｙとしている。時刻ｔ＝２５〜３２の期間が、腰回転中の期間に相当する。

この実験では、腰回転の開始時点から、時間間隔ｄｔで定められる各時刻ｔ_ｉ（＝ｔ_ｉ−１＋ｄｔ）において、加速度センサー１２の検出値ｆ_ｉ、及び、ジャイロセンサー１４の検出値ω_ｉをもとに、Ｘ，Ｙ軸それぞれの成立条件式を生成した。そして、開始時刻ｔ_１から時刻ｔ_ｉまでのＸ，Ｙ軸それぞれの成立条件式を用いて、逐次近似法（逐次推定法）の１つである逐次最小二乗法によって、時刻ｔ_ｉにおける加速度センサー１２のバイアス値ｂ_ａｘｉ，ｂ_ａｙｉを算出した。時刻ｔ_ｉにおける姿勢角φ_ｉ，θ_ｉは、式（２３），（２５）に一例を示すように、ジャイロセンサー１４の検出値ω_ｉをもとに、前回時刻ｔ_ｉ−１における姿勢角φ_ｉ−１，θ_ｉ−１に対して更新して算出した。逐次最小二乗法は公知の手法であるので、詳細な説明を省略する。

この実験結果によれば、加速度バイアスｂ_ａｘ，ｂ_ａｙは、腰回転の開始直後は変動して不安定となったが、腰回転が終了する頃（時刻ｔ＝３２付近）にはほぼ一定値に落ち着いた（収束した）。最終的に、この実験結果からは、加速度バイアスｂ_ａは、Ｘ軸加速度バイアスｂ_ａｘ≒−０．６２［ｍ／ｓ／ｓ］、Ｙ軸加速度バイアスｂ_ａｙ≒−０．２［ｍ／ｓ／ｓ］、と算出された。また、この実験では、継続して腰回転をもう１回行って同様にバイアスを算出した結果、Ｘ軸加速度バイアスｂ_ａｘ≒−０．６［ｍ／ｓ／ｓ］、Ｙ軸加速度バイアスｂ_ａｙ≒−０．２［ｍ／ｓ／ｓ］、と算出された。別途求めたより正確な値が、Ｘ軸加速度バイアスｂ_ａｘ＝−０．５６［ｍ／ｓ／ｓ］、Ｙ軸加速度バイアスｂ_ａｙ＝−０．１９［ｍ／ｓ／ｓ］である。このことから、短時間で行える簡易な回転動作によって高精度に加速度バイアスを推定できるという本実施形態の効果が確認できた。

［機能構成］
図３は、位置算出装置１の機能構成を示すブロック図である。図３によれば、位置算出装置１は、センサー部１０と、操作部１１０と、表示部１２０と、音出力部１３０と、時計部１４０と、処理部２００と、記憶部３００とを備えて構成される。

センサー部１０は、慣性センサー（ＩＭＵ）を備えて構成されるセンサーユニットであり、加速度センサー１２と、ジャイロセンサー１４とを有する。センサー部１０による検出値は、処理部２００に出力される。

操作部１１０は、例えばタッチパネルやボタンスイッチ等の入力装置で実現され、なされた操作に応じた操作信号を処理部に出力する。表示部１２０は、例えばＬＣＤ等の表示装置で実現され、処理部からの表示信号に基づく各種表示を行う。音出力部１３０は、例えばスピーカー等の音出力装置で実現され、処理部からの音信号に基づく各種音出力を行う。時計部１４０は、位置算出装置１の内部時計であり、水晶発振器等を有する発振回路によって構成され、計時した現在時刻や、指定タイミングからの経過時間等の時間信号を、処理部２００に出力する。

処理部２００は、例えばＣＰＵ等の演算装置で実現され、記憶部３００に記憶されたプログラムやデータ、操作部１１０からの操作信号等に基づいて、位置算出装置１の全体制御を行う。処理部２００は、センサー部１０からの検出値を、初期設定で求めた加速度バイアスデータ３５０で補正し、補正後の値を用いて位置算出し、算出した位置を表示部１２０に表示させる。本実施形態では、加速度バイアスデータ３５０を求めるための機能部として、処理部２００は、加速度バイアス算出部２１０を含む。

加速度バイアス算出部２１０は、姿勢角判定部２１２と、成立条件式生成部２１４と、バイアス算定部２１６と、バイアス決定部２１８と、報知制御部２２０とを有し、センサー部１０の検出値をもとに、加速度センサー１２の各検出軸（Ｘ，Ｙ，Ｚ軸）のバイアス値ｂ_ａ＝（ｂ_ａｘ，ｂ_ａｙ，ｂ_ａｚ）を算出する。

姿勢角判定部２１２は、静止時に、加速度センサー１２の検出値ｆ＝（ｆ_ｘ，ｆ_ｙ，ｆ_ｚ）をもとに、式（４）に従って、静止時の姿勢角である初期姿勢角φ_０，θ_０を算出する。それ以降は、直前の時刻ｔ_ｉ−１における姿勢角φ_ｉ−１，θ_ｉ−１と、ジャイロセンサー１４の検出値ω_ｉ＝（ω_ｘｉ，ω_ｙｉ，ω_ｚｉ）とをもとに、式（２３），（２５）に従って、現在時刻ｔ_ｉにおける姿勢角φ_ｉ，θ_ｉを算出する。算出した初期姿勢角φ_０，θ_０は、初期姿勢データ３３０として記憶される。また、時刻ｔ_ｉにおける姿勢角φ_ｉ，θ_ｉは、時刻ｔ_ｉと対応付けて、加速度バイアス算出用データ３４０に蓄積記憶される。初期姿勢角φ_０，θ_０を算出することと、時刻ｔ_ｉにおける姿勢角φ_ｉ，θ_ｉを算出することとが、姿勢角を判定することに相当する。

図４は、加速度バイアス算出用データ３４０のデータ構成例である。図４に示すように、加速度バイアス算出用データ３４０は、時刻３４１毎に生成され、加速度センサー１２の検出軸３４２それぞれについて、姿勢角３４３と、成立条件式の係数３４４と、加速度バイアス３４５とを対応付けて格納している。

成立条件式生成部２１４は、加速度センサー１２の検出値ｆ_ｉ＝（ｆ_ｘｉ，ｆ_ｙｉ，ｆ_ｚｉ）と、ジャイロセンサー１４の検出値ω_ｉ＝（ω_ｘｉ，ω_ｙｉ，ω_ｚｉ）と、姿勢角φ_ｉ，θ_ｉとをもとに、式（２７），（３１）に従って、時刻ｔ_ｉにおける瞬時成立条件であるＸ，Ｙ軸の成立条件式を算出する。算出したＸ，Ｙ軸それぞれの成立条件式における係数Ａ〜Ｄは、時刻ｔ_ｉと対応付けて、加速度バイアス算出用データ３４０に蓄積記憶される。

バイアス算定部２１６は、時刻ｔ_ｉにおける加速度バイアスｂ_ａｉを算出する。具体的には、時刻ｔ_１から時刻ｔ_ｉまでの各時刻におけるＸ，Ｙ軸それぞれの成立条件式を連立方程式とし、この連立方程式に対する所定の近似演算（例えば、最小二乗法）を行うことで、時刻ｔ_ｉにおけるＸ，Ｙ軸のバイアス値ｂ_ａｘ，ｂ_ａｙを算出する。また、加速度センサー１２のＺ軸の検出値ｆ_ｚｉと、姿勢角φ_ｉ，θ_ｉとをもとに、式（３）に従って、Ｚ軸のバイアス値ｂ_ａｚｉを算出する。算出したバイアス値ｂ_ａ＝（ｂ_ａｘ，ｂ_ａｙ，ｂ_ａｚ）は、時刻ｔ_ｉと対応付けて、加速度バイアス算出用データ３４０に蓄積記憶される。

バイアス決定部２１８は、回転中の各時刻ｔ_ｉにおける加速度バイアスｂ_ａｉから、最終的な加速度バイアスを決定する。すなわち、図２に示したように、加速度バイアスｂ_ａは、ある程度の時間経過、すなわち、連立方程式を構成するＸ，Ｙ軸それぞれの成立条件式の数の増加に伴って、一定値に収束する。このため、回転中の加速度バイアスｂ_ａｉの時間経過に伴う収束を判断し、収束した値を、最終的な加速度バイアスとして決定する。収束の判断は、例えば、所定時間の間の加速度バイアスの変動幅が所定範囲内に収まっている場合に収束したと判断してもよいし、各時刻ｔ_ｉの加速度バイアスの値の時間微分を求め、この時間微分の値がゼロ近傍を示す所定条件を連続して所定回数満足した場合に収束したと判断してもよい。決定した加速度バイアスは、加速度バイアスデータ３５０として記憶される。ここで決定した加速度バイアスｂ_ａ＝（ｂ_ａｘ，ｂ_ａｙ，ｂ_ａｚ）を用いて、当該装置の位置算出が行われる。

図５は、加速度バイアスデータ３５０のデータ構成例である。図５に示すように、加速度バイアスデータ３５０は、加速度センサー１２の検出軸３５１それぞれのバイアス値３５２を格納している。

報知制御部２２０は、加速度バイアスの算出の際に、ユーザーに向けた必要な報知制御を行う。具体的には、初期姿勢角φ_０，θ_０の和が所定角度（例えば、１０度）を超える場合に（φ_０＋θ_０＞１０度）、当該装置を推奨姿勢となるように再装着することをユーザーに促す所定の報知メッセージを音出力部１３０から音声出力させたり、所定の報知ランプや報知メッセージを表示部１２０に表示出力させたりする。また、瞬時バイアスｂ_ａｉを取得するために、当該装置を装着した状態での左右方向への腰回転をユーザーに促す所定の報知メッセージを音出力部１３０から音声出力させたり、所定の報知ランプや報知メッセージを表示部１２０に表示出力させたりする。また、バイアス決定部２１８が、収束を判断して加速度バイアスを決定できた場合にも報知制御を行い、加速度バイアスの決定・設定に係る初期設定が完了したことをユーザーに知らせる。

記憶部３００は、ＲＯＭやＲＡＭ等の記憶装置で実現され、処理部２００が位置算出装置１を統合的に制御するためのプログラムやデータ等を記憶しているとともに、処理部２００の作業領域として用いられ、処理部２００が各種プログラムに従って実行した演算結果や、センサー部１０からの検出値データ等が一時的に格納される。本実施形態では、記憶部３００には、システムプログラム３１０と、加速度バイアス算出プログラム３１２と、センサー検出データ３２０と、初期姿勢データ３３０と、加速度バイアス算出用データ３４０と、加速度バイアスデータ３５０とが記憶される。

センサー検出データ３２０は、センサー部１０による検出値の蓄積データであり、加速度センサー１２の検出値（加速度）ｆの蓄積データである加速度検出データ３２２と、ジャイロセンサー１４の検出値（角速度）ωの蓄積データであるジャイロ検出データ３２４とを含んでいる。

［処理の流れ］
図６は、加速度バイアス推定処理の流れを説明するフローチャートである。この処理は、位置算出処理に先立ち、加速度バイアス算出部２１０が、加速度バイアス算出プログラム３１２に従って実行する処理である。例えば、初期設定として実行したり、校正を必要とする任意のタイミングで実行する処理である。

先ず、姿勢角判定部２１２が、加速度センサー１２の検出値ｆ＝（ｆ_ｘ，ｆ_ｙ，ｆ_ｚ）をもとに、初期姿勢φ_０，θ_０を算出する（ステップＳ１）。そして、算出した初期姿勢角φ_０，θ_０の和が１０度以下となっているかを判断する（ステップＳ３）。１０度を超えているならば（ステップＳ３：ＮＯ）、報知制御部２２０が、ユーザーに対して、推奨姿勢となるように当該装置の再装着を促す所定の報知を行う（ステップＳ５）。その後、再度、姿勢角判定部２１２が、初期姿勢角φ_０，θ_０を算出する（ステップＳ１）。

初期姿勢φ_０，θ_０の和が１０度以下となっているならば（ステップＳ３：ＹＥＳ）、報知制御部２２０が、ユーザーに対して、腰の左右回転を促す所定の報知を行う（ステップＳ７）。

その後、所定の時間間隔ｄｔで、ループＡの処理を繰り返し実行する。ループＡでは、先ず、時刻ｔ_ｉにおける加速度センサー１２の検出値ｆ_ｉ＝（ｆ_ｘｉ，ｆ_ｙｉ，ｆ_ｚｉ）と、ジャイロセンサー１４の検出値ω_ｉ＝（ω_ｘｉ，ω_ｙｉ，ω_ｚｉ）とを取得する（ステップＳ９）。次いで、姿勢角判定部２１２が、ジャイロセンサー１４の検出値ω_ｉをもとに、時刻ｔ_ｉにおける姿勢角φ_ｉ，θ_ｉを算出する（ステップＳ１１）。

続いて、成立条件式生成部２１４が、加速度センサー１２の検出値ｆ_ｉと、ジャイロセンサー１４の検出値ω_ｉと、時刻ｔ_ｉにおける姿勢角φ_ｉ，θ_ｉとをもとに、時刻ｔ_ｉにおけるＸ，Ｙ軸それぞれの成立条件式を生成する（ステップＳ１３）。そして、生成したＸ，Ｙ軸それぞれの成立条件式を追加して連立方程式を更新し、更新した連立方程式に対する所定の近似計算（例えば、最小二乗法）を行うことで、時刻ｔ_ｉにおけるバイアス値ｂ_ａｘｉ，ｂ_ａｙｉを算出する（ステップＳ１５）。また、加速度センサー１２の検出値ｆ_ｉと、姿勢角φ_ｉ，θ_ｉとをもとに、Ｚ軸の加速度バイアス値ｂ_ａｚｉを算出する（ステップＳ１７）。

続いて、バイアス決定部２１８が、開始時刻ｔ_１から現在時刻ｔ_ｉまでの各時刻において算出した加速度バイアスｂ_ａｉが一定値範囲に収束したかを判断する（ステップＳ１９）。収束していないならば（ステップＳＳ１９：ＮＯ）、ループＡの処理を繰り返す。収束したならば（ステップＳ１９：ＹＥＳ）、ループＡの処理を終了した後、加速度バイアス値ｂ_ｉの収束値を、最終的な加速度バイアス値として決定し、加速度バイアスデータ３５０に設定する（ステップＳ２１）。そして、加速度バイアス算出処理を終了する。

［作用効果］
このように、本実施形態によれば、ＩＭＵ１０を有する位置算出装置１をユーザー２０の腰部に装着し、ユーザー２０が、左右方向に腰回転するといった簡易な動作で、加速度センサー１２のバイアス値ｂ＝（ｂ_ａｘ，ｂ_ａｙ，ｂ_ａｚ）を算出することができる。位置算出装置１は、加速度センサー１２のＺ軸を鉛直下向きに向け、Ｘ−Ｙ平面が略水平となるように、ユーザー２０に装着すればよい。

なお、上述の実施形態において、加速度センサー１２を回転させた場合、Ｚ軸と回転軸とがほぼ一致するならば、重力以外の回転力を無視でき、静止時と同様に鉛直下向きの重力のみが作用しているとみなすことができるとした。このことについて、詳細に説明する。加速度センサー１２を回転させた場合にＸ−Ｙ平面方向に作用する遠心加速度ａは、回転加速度ω、及び、回転半径Ｒ、によって決まり、ａ＝ω^２・Ｒ、で与えられる。本願発明者らの実験結果から、遠心加速度ａが重力加速度ｇの０．１倍以下（ａ≦０．１ｇ）ならば、遠心加速度ａを無視しても、加速度バイアスの算出の際に生じる誤差が無視できることがわかっている。

ユーザー２０が腰部に位置算出装置１（加速度センサー１２）を装着して腰回転を行った場合の、回転半径Ｒ及び回転加速度ωの上限について考えてみる。例えば、ユーザー２０が、成人の平均腰囲８５ｃｍを有する場合、回転半径Ｒは、Ｒ＝１３ｃｍ、程度となる。この場合、回転加速度ωが、ω≦２［ｒｐｓ］、ならば、遠心加速度ａは、ａ≦０．５２［ｍ／ｓ／ｓ］、であり、無視できる。しかし、回転加速度ωが、ω≧３［ｒｐｓ］、ならば、遠心加速度ａは、ａ≧１．１７［ｍ／ｓ／ｓ］、となり、無視できない程度の大きさとなる。つまり、普通体型のユーザー２０ならば、１〜２［ｒｐｓ］程度の回転角度ωで腰回転を行えば良い。

［変形例］
以上、本発明の実施形態を説明したが、本発明が適用可能な形態は、上述した実施形態に限らない。

例えば、加速度センサー１２のＺ軸を鉛直方向とすることとして説明したが、Ｘ軸やＹ軸を鉛直方向としてもよい。直交３軸のうち、１軸が鉛直方向であれば、他の２軸がなす平面が水平面となるからである。この場合、当該平面に含まれる他の２軸が、第１軸及び第２軸に相当する。

また、鉛直方向を軸中心として回転することの意味は、Ｚ軸が軸中心である必要はなく、Ｚ軸を鉛直方向に向けた偏心回転としてもよい。

また、位置算出装置１をユーザー２０の腰部に装着して使用することとして説明したが、胸部などの他の胴体部や、頭部、或いは脚部に装着して用いることとしてもよい。加速度バイアスの算出において、Ｚ軸を鉛直方向に沿った方向とした状態で、鉛直方向を軸中心として回転できれば装着位置は任意である。

また、各時刻ｔ_ｉで求めたＸ，Ｙ軸それぞれの成立条件式を用いて、最小二乗法で加速度バイアスを算出することとして説明したが、他の近似計算（推定計算）を用いることとしてもよい。例えば、カルマンフィルターや、最尤推定法を用いて加速度バイアスを算出してもよい。

また、腰を左右方向に回転させる動作を行うこととして説明したが、鉛直方向を軸中心として位置算出装置１を３６０度回転させても良い。位置算出装置１の使用時に対して条件変化が少ないので、回転はユーザー２０に装着して行うことが望ましいが、略水平な地面などに置いた状態で回転させても良い、鉛直方向を軸中心として回転ができれば、回転のさせ方は任意である。

また、位置算出装置１を例として説明したが、携帯型のスポーツ機器、携帯型電話機やパソコン、ＰＤＡ（Personal Digital Assistant）といった他の電子機器についても本発明を同様に適用して、加速度センサー１２のバイアスを算出することが可能である。

１ 位置算出装置、１０ センサー部、１２ 加速度センサー、１４ ジャイロセンサー、１１０ 操作部、１２０ 表示部、１３０ 音出力部、１４０ 時計部、２００ 処理部、２１０ 加速度バイアス算出部、２１２ 姿勢角判定部、２１４ 成立条件式生成部、２１６ バイアス算定部、２１８ バイアス決定部、２２０ 報知制御部、３００ 記憶部、３１０ システムプログラム、３１２ 加速度バイアス算出プログラム、３２０ センサー検出データ、３２２ 加速度検出データ、３２４ ジャイロ検出データ、３３０ 初期姿勢データ、３４０ 加速度バイアス算出用データ、３５０ 加速度バイアスデータ、３ 操作スイッチ、５ ディスプレイ、７ スピーカー、２０ ユーザー

Claims (8)

1. 加速度センサーとジャイロセンサーとを有するセンサー部を、前記加速度センサーの第１軸及び第２軸がなす平面を鉛直方向に対して略垂直にした状態で、前記鉛直方向を軸中心に回転させることと、
   前記回転中に得られた前記センサー部の検出値を用いて前記加速度センサーの前記第１軸及び前記第２軸のバイアスを算出することと、
   を含む加速度バイアス算出方法。
2. 前記バイアスを算出することは、
   前記バイアスを未知数として前記センサー部の姿勢を前記検出値を用いて表した瞬時成立条件を、前記回転中に複数算出することと、
   複数の前記瞬時成立条件を用いて前記バイアスを算定することと、
   を含む請求項１に記載の加速度バイアス算出方法。
3. 前記センサー部の検出値を用いて前記センサー部の姿勢角を判定することを更に含み、
   前記瞬時成立条件を算出することは、前記検出値と、前記姿勢角と、重力加速度とを用いて前記瞬時成立条件を算出することである、
   請求項２に記載の加速度バイアス算出方法。
4. 前記姿勢角を判定することは、前記センサー部を静止させた状態での前記加速度センサーの検出値と、重力加速度とを用いて初期の姿勢角を判定することと、従前に判定した姿勢角を前記ジャイロセンサーの検出値を用いて更新することで複数の前記姿勢における姿勢角を判定することとを含む、
   請求項３に記載の加速度バイアス算出方法。
5. 前記バイアスを算定することは、複数の前記瞬時成立条件を用いた所定の近似計算を行って前記バイアスを算定することである、
   請求項２〜４の何れか一項に記載の加速度バイアス算出方法。
6. 前記回転させることは、前記第１軸と前記鉛直方向に対して垂直な水平面とがなす角度と、前記第２軸と前記水平面とがなす角度との和が１０度以下の状態で、前記センサー部を回転させることである、
   請求項１〜５の何れか一項に記載の加速度バイアス算出方法。
7. 前記センサー部は、ユーザーの胴体部、頭部、或いは脚部に装着されて用いられる、
   請求項１〜６の何れか一項に記載の加速度バイアス算出方法。
8. 加速度センサーとジャイロセンサーとを有するセンサー部の前記加速度センサーのバイアスを算出するバイアス算出装置であって、
   前記加速度センサーの第１軸及び第２軸がなす平面を鉛直方向に対して略垂直にした状態で、前記鉛直方向を軸中心にして前記センサー部を回転させる旨の報知を行う報知制御部と、
   前記回転中に得られた前記センサー部の検出値を用いて前記加速度センサーの前記第１軸及び前記第２軸のバイアスを算出するバイアス算出部と、
   を備えたバイアス算出装置。

Images