加速度センサの補正装置および加速度センサの出力値補正方法
姿勢角演算器（１４）は、加速度センサ（１０）の出力値からロボットの姿勢角を演算する。姿勢角比較器（１６）は、レジスタ（２０）に設定された特定姿勢における姿勢角と検出された姿勢角とを比較し、その差を補正値演算器（１８）に出力する。補正値演算器（１８）は差を解消するように零点補正器（２６）あるいは感度補正器（２８）に補正値を出力する。レジスタ（２０）に設定される姿勢角は、入力器（２２）から設定してもよい。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明はロボット等の運動体に設けられる加速度センサの出力値を補正する技術に関する。
【背景技術】
【０００２】
ロボット等の移動体の姿勢制御に加速度センサやヨーレートセンサが用いられている。直交する３軸をｘ軸、ｙ軸、ｚ軸とすると、各軸方向の加速度を３個の加速度センサで検出し、各軸回りのヨーレートを３個のヨーレートセンサで検出する。軸回りの角度、あるいは姿勢角（ピッチ角、ロール角、ヨー角）は、ヨーレートセンサの出力を時間積分して得られる。
【０００３】
特開２００４−２６８７３０号公報には、ジャイロセンサから出力される加速度データ及び姿勢データを用いて姿勢制御する技術が開示されている。
【０００４】
加速度センサには零点オフセットが存在し、運動体の静止時に零点オフセットを補正することが必要であるが、静止時においても重力加速度があるため、零点を決定することができない。もちろん、零点安定性や精度の高い加速度センサを用いればよいが、高価でサイズや重量も大きくなる。
【発明の開示】
【発明が解決しようとする課題】
【０００５】
本発明の目的は、簡易な構成で加速度センサの出力値を補正し、高精度に加速度、ひいては運動体の姿勢角を検出できる技術を提供することにある。
【課題を解決するための手段】
【０００６】
本発明の第１の態様は、運動体に設けられた加速度センサからの出力値に基づき運動体の姿勢角データを演算する手段と、前記姿勢角データと基準姿勢角データとを比較することで前記加速度センサの出力値を補正する手段とを有する加速度センサの補正装置に関する。
【０００７】
この補正装置によれば、加速度センサの出力値からロボット等の運動体の姿勢角データが演算される。この姿勢角データを加速度センサによる検出とは別個に検出され、あるいは設定された基準姿勢角と前記演算された姿勢角データとが比較される。加速度センサの出力値に零点オフセットあるいは感度異常が存在する場合、出力値に基づき演算された姿勢角は、基準姿勢角データと異なる値を示す。そこで、両姿勢角データを比較することで、加速度センサの出力値の異常、及びその度合いを検出して補正することができる。上記補正装置では、加速度センサで検出された加速度自体ではなく、加速度から得られる姿勢角データ同士を比較するため、重力加速度の影響によらず高精度に補正できる。
【０００８】
本発明によれば、簡易な構成で加速度センサの出力値を補正し、高精度に加速度や運動体の姿勢角を検出できる。
【０００９】
本発明の第２の態様は、加速度センサの出力値を補正する方法に関する。この方法は、加速度センサからの出力値に基づき運動体の姿勢角データを演算するステップと、前記姿勢角データと基準姿勢角データとを比較するステップと、前記姿勢角データと基準姿勢角データとの比較結果に基づき、前記加速度センサの出力値を補正するステップとを有する。
【発明を実施するための最良の形態】
【００１０】
以下、図面に基づき本発明の実施形態について説明する。
【００１１】
＜第１実施形態＞
図１に、本実施形態の構成ブロック図を示す。加速度センサ１０は、ロボット等の運動体の所定位置に所定の姿勢で設けられ、運動体の加速度を検出して補正演算器１２に出力する。
【００１２】
補正演算器１２は、後述する零点補正器２６及び感度補正器２８からの補正データに基づいて加速度センサ１０の出力値を補正し、出力器２４に出力する。また、補正演算器１２は、補正した出力値を姿勢角演算器１４に出力する。
【００１３】
姿勢角演算器１４は、補正演算器１２からの出力値に基づき傾斜角を演算し、傾斜角に基づき姿勢行列を演算し、この姿勢行列に基づき運動体の姿勢角を演算する。加速度から傾斜角の演算、及び傾斜角から姿勢角の演算については後述する。姿勢角演算器１４は、演算して得られた姿勢角を姿勢角比較器１６に出力する。
【００１４】
姿勢角比較器１６は、出力値から得られた姿勢角（加速度姿勢角）と、レジスタ２０に設定された姿勢角（基準姿勢角）とを比較し、その差が所定の許容値以上か否かを判定する。加速度姿勢角と基準姿勢角との差が所定の許容値以上である場合には、出力値を補正する必要があると判断して、加速度姿勢角と基準姿勢角との差を補正値演算器１８に出力する。
【００１５】
補正値演算器１８は、入力した差を用いて出力値の零点及び感度を補正するために必要な補正値を演算し、それぞれ零点補正器２６及び感度補正器２８に出力する。零点補正器２６は、補正演算器１２に対して零点補正に必要な零点オフセット値を補正演算器１２に出力する。補正演算器１２は、出力値から零点オフセットを除去することで出力値を補正する。また、感度補正器２８は、補正演算器１２に対して感度補正に必要な係数（ゲイン）を補正演算器１２に出力する。出力値の補正は、零点補正器２６による零点補正のみでもよい。
【００１６】
加速度姿勢角と比較されるべき基準姿勢角は、上記のようにレジスタ２０に設定される。レジスタ２０に設定される基準姿勢角は、予めロボットを特定の姿勢に維持したときの姿勢角であるが、精度が確保されている限り、加速度センサ１０とは別個にロボットに設けられた姿勢角センサから入力器２２を介して供給してもよい。予め定めた姿勢において加速度姿勢角と基準姿勢角とを比較する場合、レジスタ２０に固定値を設定しておけばよく、入力器２２は必須でない。別個の姿勢角センサは、例えば光ファイバジャイロ（ＦＯＧ）等を用いることができる。光ファイバジャイロで得られた角速度は時間積分されて姿勢角が検出され、この姿勢角が入力器２２に供給されてレジスタ２０に設定される。加速度センサ１０が鉛直方向の加速度を検出し、運動体であるロボットが直立して静止している場合、直立時の基準姿勢角がレジスタ２０に設定され、加速度姿勢角と比較される。加速度センサ１０が正確に１Ｇを出力していれば加速度姿勢角と基準姿勢角とが所定の許容値の範囲内で一致するが、そうではない場合、その差に応じて加速度センサ１０の出力値を補正する。ロボットが傾いている場合、鉛直軸以外にも加速度成分が生じるが、そのときの加速度姿勢角と基準姿勢角とを比較することで、加速度センサ１０の出力値を補正できる。
【００１７】
姿勢角比較器１６では、加速度姿勢角と基準姿勢角を比較しているが、姿勢角演算器１４で演算された傾斜角をレジスタ２０に設定された基準姿勢角と比較してもよく、あるいは姿勢角演算器１４で演算された姿勢行列をレジスタ２０に設定された基準姿勢行列と比較してもよい。さらに、姿勢角演算器１４で姿勢角の４元数を演算し、この４元数をレジスタ２０に設定された基準４元数と比較してもよい。姿勢角、傾斜角、姿勢行列、４元数を本実施形態では「姿勢データ」と総称する。
【００１８】
以下、加速度から傾斜角を演算する方法、傾斜角から姿勢行列を演算する方法、姿勢行列から姿勢角を演算する方法について説明する。
【００１９】
まず、姿勢行列について説明する。基準座標系ＸＹＺにおけるセンサ座標系の表記法として、離散時間ｎにおける姿勢行列Ｔ（ｎ）で表す。姿勢行列Ｔ（ｎ）は、（１）式に示すように４×４の要素から構成される。
【００２０】
【数１】
【００２１】
行列Ｔ（ｎ）の意味として、第一列（ａ，ｂ，ｃ）、第二列（ｄ，ｅ，ｆ）、第三列（ｇ，ｈ，ｉ）は、それぞれ基準座標系からみたセンサ座標系ｎのｘ軸、ｙ軸、ｚ軸の方向ベクトルを表す。第四列は、基準座標系におけるセンサ座標系ｎの原点位置を表す（一般に並進が有る場合はこの第四列に並進量が表される）。原点の移動がない場合は、位置の変換を表す第４列の１〜３行目の要素を０とおく。図５に示すように、センサ座標系ｎの原点Ｏｎが、基準座標系において（０，０，０）の位置にあり、ｘ軸ベクトルは基準座標系上の（ａ，ｂ，ｃ）、ｙ軸ベクトルは（ｄ，ｅ，ｆ）、ｚ軸ベクトルは（ｇ，ｈ，ｉ）の成分を持つ。
【００２２】
姿勢角（ロール・ピッチ・ヨー角）から姿勢行列Ｔ（ｎ）を求める手法を以下に説明する。姿勢行列Ｔ（ｎ）を表すために行列による回転変換は、回転軸についての順序を考慮する必要がある。図６に示すように、ロボットで一般的に用いられるロール・ピッチ・ヨー角を用いる場合は、最初にｘ軸周りの回転φ、次に回転後のｙ軸周りの回転θ、最後に回転後のｚ軸周りの回転ψの３回の回転が生じたと定義する（軸の回転順番が固定されている点に注意）。
【００２３】
ロール・ピッチ・ヨー角による変換行列をＲＰＹ（φ、θ、ψ）とする。ＲＰＹ（φ、θ、ψ）は回転変換行列を左から右に掛けた行列の積となり、式（２）で表される。
【数２】
【００２４】
式（２）は具体的に式（３）で表される。
【数３】
【００２５】
式（３）を書き下すと、式（４）で表記できる。
【数４】
【００２６】
なお、ロール・ピッチ・ヨー角の代わりに、オイラー角を姿勢角として用いることもできる。オイラー角では、最初にｘ軸周りの回転φ、次に回転後のｙ軸周りの回転θ、最後に回転後のｚ軸周りの回転ψが起こった時の変換行列をＥｕｌｅｒ（Ｅφ、Ｅθ、Ｅψ）とおき、式（５）で表される。
【数５】
【００２７】
式（５）は具体的に式（６）で表される。
【００２８】
【数６】
【００２９】
式（６）を書き下すと、式（７）で表記できる。
【００３０】
【数７】
【００３１】
基準座標系をＯ−ＸＹＺ、初期のセンサ座標系をＯ0−ｘ0ｙ0ｚ0とおく。基準座標系と時刻ｔ＝０時の座標系Ｏ0−ｘ0ｙ0ｚ0の関係を、座標変換Ａ（０）で関係づける。時刻ｔ＝ｔn時の座標系をＯn−ｘnｙnｚnとおく。各座標系の原点Ｏ，Ｏ0，Ｏnは位置の移動がなく同一とする。その後運動体の姿勢変化により、図７に示すように、座標系Ｏ(n-1)−ｘ(n-1)ｙ(nー１)ｚ(n-1)からＯn−ｘnｙnｚnへ変化したとき、Ｏ(n-1)−ｘ(n-1)ｙ(nー１)ｚ(n-1)とＯn−ｘnｙnｚnは出力値から求まる行列Ａ（ｎ）で関係づけられる。基準座標系からみたセンサ座標系Ｔ（ｎ）は、変換Ａ（ｎ）を右からかけていくことで式（８）で求める。センサ座標系の原点が時間と共に移動する場合は、行列Ａの第４列の１〜３行目の要素に時間と共に移動した座標が逐次入れられる。行列Ａの第４列はセンサ座標系の回転に対しては影響を与えないので、ここでは特に述べない。
【００３２】
【数８】
【００３３】
次に、光ファイバジャイロ等の角速度出力値から微小回転行列Ａ（ｎ）行列を導出する方法について説明する。３個の角速度センサは、センサ座標系の各軸に設置されており、図８に示すように、センサｘ，ｙ，ｚ軸まわりの角速度を計測している。式（４）において、回転角Δφ，Δθ，Δψが十分小さいとき、
【数９】
【数１０】
である。これよりセンサｘ軸回りの微小回転角Δφ、センサｙ軸回りの微小回転角Δθ、センサｚ軸回りの微小回転角Δψを用いて式（１１）で表すことができる。式（１１）の結果は行列の各要素が独立に各微小回転角で表せるため、回転の順番に依存しないと近似している。
【００３４】
【数１１】
【００３５】
微小角と出力値との間には、微小回転角Δφ、Δθ、Δψ、角速度センサからの出力値ωｘ，ωｙ，ωｚ，サンプリング周期ｔｓから、式 （１２）〜（１４）の関係がある。サンプリング周期ｔｓが回転運動に対して充分早い周期としているため、サンプリング周期ｔｓの時間内での回転は充分小さく、微小回転角とみなすことができる。
【００３６】
【数１２】
【数１３】
【数１４】
【００３７】
このためＡ（ｎ）行列は式（１５）で表される。
【数１５】
【００３８】
次に、姿勢行列から姿勢角を求める手法について述べる。
【００３９】
姿勢行列Ｔ（ｎ）が式（１６）で表されている。
【数１６】
【００４０】
ヨー角ψは、
【数１７】
姿勢角であるヨー角ψの変域は、−π＜ψ≦πである。
【００４１】
ロール角φは、
【数１８】
姿勢角であるロール角φの変域は、−π／２≦φ≦π／２である。
【００４２】
ピッチ角θは、
【数１９】
姿勢角であるピッチ角θの変域は、 −π＜θ≦πである。
【００４３】
オイラー角を用いる場合は、式（２０）〜（２３）を使う。
【数２０】
【数２１】
【数２２】
【数２３】
【００４４】
次に、行列の正規化について説明する。
【数２４】
【００４５】
姿勢行列Ｔ（ｎ）では、演算後、姿勢行列の各列が単位ベクトルとならないことがあるため式（２４）の各列ベクトルの大きさが１になるように式（２５）で正規化を行う。
【００４６】
【数２５】
【００４７】
ここでｐ1，ｐ2は式（２６）、（２７）で与えられる。
【数２６】
【数２７】
【００４８】
その後、正規化後の各要素をあらためて、
【数２８】
と置き直す。
【００４９】
さらに、行列の直交化について説明する。
【数２９】
【００５０】
姿勢行列Ｔ（ｎ）では演算後、姿勢行列の各列が直交した軸とならないことがあるため、式（２９）の各列ベクトルが直交する直交化処理を行う（この場合、ｚ軸を基準としている）。ｚ軸、ｙ軸に直交する新しいｘ’軸を得るため、ａ’，ｂ’，ｃ’を求める。
【００５１】
【数３０】
【数３１】
【数３２】
【００５２】
次に、ｚ軸、ｘ’軸に直交する新しいｙ’軸を得るために、ｄ’，ｅ’，ｆ’を求める。
【数３３】
【数３４】
【数３５】
【００５３】
求めたａ’〜ｆ’から、直交化した姿勢行列Ｔ（ｎ）を得る。
【数３６】
【００５４】
ここで、ａｔａｎ２について説明する。ａｔａｎ２（ｙ，ｘ）は、２変数ｘ，ｙを持つ計算機用関数である。通常使われるａｔａｎ関数よりも適用範囲が広い。
【数３７】
【００５５】
（−π＜ξ≦π）
は、
ｘ＞０、ｙ＞０の時
【数３８】
となり、
ｘ＞０、ｙ＜０の時
【数３９】
となる。同様にして、
ｘ＜０，ｙ＞０の時
ξ＝π＋ｔａｎ-1（ｙ／ｘ）
となり、
ｘ＜０，ｙ＜０の時
ξ＝−π＋ｔａｎ-1（ｙ／ｘ）
となり、
ｘ＝０，ｙ＞０の時
ξ＝π／２
となり、
ｘ＝０，ｙ＜０の時
ξ＝−π／２
となり、
ｘ＝０，ｙ＝０の時
ξ＝０
となる。
【００５６】
次に、傾斜角の演算について説明する。加速度センサ１０からの加速度に基づき、姿勢角演算器１４で傾斜角を演算する方法である。傾斜角とは、センサｘ、ｙ、ｚ軸と基準Ｚ軸との間の角度λｘ、λｙ、λｚである。すなわち、
λｘ：ｘ軸とＺ軸の間の角度
λｙ：ｙ軸とＺ軸の間の角度
λｚ：ｚ軸とＺ軸の間の角度
であり、λｘ、λｙ、λｚの範囲は、０≦（λｘ、λｙ、λｚ）≦πである。図９に、傾斜角と重力ベクトルを示す。センサ座標に配置された加速度センサから以下のように傾斜角を求める。加速度Ｇｘ、Ｇｙ、Ｇｚを式（４０）〜（４２）用いて正規化し、正規化後の加速度Ｇｘ’、Ｇｙ’、Ｇｚ’を求める。
【００５７】
【数４０】
【数４１】
【数４２】
【００５８】
加速度Ｇｘ’、Ｇｙ’、Ｇｚ’から、式（４３）〜（４５）を用いて傾斜角λｘ、λｙ、λｚを求める。
【００５９】
【数４３】
【数４４】
【数４５】
【００６０】
次に、傾斜角λｘ、λｙ、λｚ から姿勢行列Ｔ（ｎ）を求める手法について述べる。姿勢角演算器１４で傾斜角に基づき姿勢行列を求める演算である。
【００６１】
【数４６】
【数４７】
【数４８】
【数４９】
【数５０】
【数５１】
【数５２】
【数５３】
【数５４】
【数５５】
【数５６】
【００６２】
以上の結果から、姿勢行列Ｔ（ｎ）を求める。
【００６３】
なお、姿勢行列Ｔ（ｎ）から傾斜角λｘ、λｙ、λｚを求める際には以下の式を用いる。
【数５７】
【数５８】
【数５９】
【数６０】
【００６４】
このように、本実施形態では、加速度センサ１０で得られた姿勢角と、別個の姿勢角センサで得られた基準姿勢角とを比較することで、加速度センサの出力値を簡易に補正することができる。
【００６５】
＜第２実施形態＞
図２に、本実施形態の構成ブロック図を示す。図１と異なる点は、加速度センサ１０として加速度センサ１０ａ、１０ｂ、１０ｃが３個設けられてｘ、ｙ、ｚ各軸方向の加速度を検出し、また、加速度センサ１０ａ、１０ｂ、１０ｃに対応して補正演算器１２ａ、１２ｂ、１２ｃが設けられる点である。
【００６６】
３個の加速度センサ１０ａ、１０ｂ、１０ｃにより加速度を検出し、これらの出力値から姿勢角を演算することで、運動体の姿勢を一義的に特定することができる。運動体の姿勢を順次変化させて特定の姿勢を実現し、これら特定の姿勢において検出された姿勢角をレジスタ２０に設定された基準姿勢角と比較する。例えば、ロボットの姿勢を順次変化させてｘ軸、ｙ軸、ｚ軸が順次Ｚ軸方向（鉛直方向）を向くようにし、そのときの加速度姿勢角と基準姿勢角との差を用いて各加速度センサ１０ａ、１０ｂ、１０ｃの出力値を順次補正する。加速度センサ１０ａ、１０ｂのみでもよく、一般に複数ｎ個（ｎ≧２）の加速度センサを設けることができる。
【００６７】
なお、図では便宜上、零点補正器２６からの補正信号は補正演算器１２ａのみに出力されているが、他の補正演算器１２ｂ、１２ｃにも補正信号が出力されてもよい。感度補正器２８についても同様である。
【００６８】
＜第３実施形態＞
図３に、本実施形態の構成を示す。上記の各実施形態において、出力値の補正はロボットが特定の姿勢で静止している場合に実行される。したがって、例えば、ユーザあるいはロボットのメインプロセッサからの指示により加速度センサの補正装置が補正を実行する構成の場合、補正実行命令を受けたときにロボットが静止しており補正を実行できるタイミングであるか否かを判定する必要がある。図３における静止判定器３０は、外部からの補正実行命令を受けてロボットが静止状態にあるか否かを判定する。
【００６９】
静止判定器３０は、姿勢角演算器１４からの姿勢角の変化量を検出し、この変化量が所定値以下であるか否かを判定する。姿勢角の変化量が所定値以下である場合にはロボットは静止状態にあると判定し、補正値演算器１８に補正許可信号を出力する。補正値演算器１８は、補正許可信号を受信することで補正値を演算し、零点補正器２６等に出力する。静止判定器３０は、姿勢角の変化量ではなく、加速度センサ１０からの出力値自体の変化量を検出し、この変化量を所定値と比較して静止状態を判定してもよい。ロボットが静止しておらず運動している場合、並進加速度及び遠心加速度が重畳し、かつ、補正されるべき出力値が時間とともに変化するため補正精度が著しく低下する。ロボットの静止状態において出力値の補正を実行することで、補正精度を確保できる。
【００７０】
図４に、本実施形態の処理フローチャートを示す。まず、ユーザ（あるいはユーザの指示を受けたメインプロセッサ）からの補正命令を入力し、姿勢角として、ピッチ角θｉ、ロール角φｉ、及びヨー角yｉを入力する（Ｓ１０１）。ユーザ（あるいはユーザの指示を受けたメインプロセッサ）から入力された基準姿勢角（φｉ、θｉ、yｉ）はレジスタ２０に設定される。静止判定器３０は、この補正命令を受信すると、加速度センサ１０ａ、１０ｂ、１０ｃからの出力値、あるいは姿勢角演算器１２ａ、１２ｂ、１２ｃからの姿勢角の変化量（時間変動幅）を検出し、所定値以下であるか否かを判定する（Ｓ１０２）。変化量が所定値以下である場合、静止判定器３０はロボットが静止状態にあると判定する。なお、変化量が所定値以下である時間を所定のしきい時間と比較し、所定のしきい時間以上である場合のみロボットが静止状態にあると判定してもよい。所定のしきい時間は例えば３秒に設定でき、これにより補正に必要な有意の静止状態を検出できる。
【００７１】
静止判定器３０でロボットが静止状態にあると判定した場合、静止判定器３０は上記のように補正許可信号を補正値演算器１８に出力する。補正値演算器１８は、この補正許可信号により、そのときの加速度姿勢角と基準姿勢角との差に基づき、この差が減少又はなくなるように補正値を演算して出力する。補正演算器１２ａ、１２ｂ、１２ｃは補正値を用いて出力値の零点補正あるいは感度補正を行う（Ｓ１０３）。
【００７２】
次に、補正を繰り返し行うか否かを判定し（Ｓ１０４）、複数回行う必要があればロボットの姿勢を変化させ（Ｓ１０５）、新たに基準姿勢角（φｊ、θｊ、yｊ）を入力して同様の補正処理を行う。３個の加速度センサ１０ａ、１０ｂ、１０ｃの全てについて補正することが好適であり、この場合に少なくとも３回補正処理を繰り返す。例えば、（０、０、０）としてｚ軸方向の加速度センサ１０ｃについて感度補正を行い、次に（π／２、０、０）としてｘ軸方向の加速度センサ１０ａについて感度補正を行い、次に（０、π／２、０）としてｙ軸方向の加速度センサ１０ｂについて感度補正を行うである。補正を実行する姿勢としては、加速度センサ１０ａ、１０ｂ、１０ｃの検出方向が基準座標系の軸に対して平行となる姿勢が好適であるが、必ずしも平行である必要はない。３個の加速度センサにおいて、ｘ軸方向、ｙ軸方向の２個の加速度センサ１０ａ、１０ｂについて補正を行い、ｚ軸方向は補正を行わずとも精度が得られる場合は、２回の補正を行えばよい。これは、ロボットの姿勢が大きく傾かない場合などが想定される。
【００７３】
上記の実施形態において、コントローラは汎用プロセッサで実現される。コントローラは、全体の制御のためのメインあるいは中央処理部、及び種々の異なる特定の計算や機能を中央処理部の制御の下に実行する独立した機能部を有する特定用途向けのＩＣ（例えばＡＳＩＣ）で実現され得ることは当業者には理解されよう。 コントローラは、専用のあるいはプログラム可能な複数の集積回路あるいはその他の電子回路や電子装置（例えば、ワイヤ接続された電子あるいは論理回路、ＰＬＤ、ＰＬＡ、ＰＡＬ等のプログラマブルロジックデバイス）でもよい。コントローラは、マイクロプロセッサ、マイクロコントローラ、あるいは他の処理装置（ＣＰＵ、ＭＰＵ）等の汎用コンピュータで使用するようにプログラムされ得る。実施形態で述べた手順を実行できる任意の装置あるいは装置群がコントローラとして使用可能である。データ／信号処理能力の最大化と高速化のために分散処理構造を用いることもできる。
【００７４】
本発明の実施形態について説明したが、本発明は上記の実施形態に限定されるものではない。本発明は種々の変形例や均等物を含む。種々の実施形態が示されているが、本発明はこれらの組み合わせも含む。
【図面の簡単な説明】
【００７５】
【図１】実施形態の構成ブロック図である。
【図２】他の実施形態の構成ブロック図である。
【図３】さらに他の実施形態の構成ブロック図である。
【図４】実施形態の補正処理フローチャートである。
【図５】基準座標系（ＸＹＺ）とセンサ座標系（ｘｙｚ）との関係を示す図である。
【図６】基準座標系における姿勢角（ロール角、ピッチ角、ヨー角）を示す図である。
【図７】センサ座標系ｎの時間的変化を示す図である。
【図８】センサ座標系における微小回転角を示す図である。
【図９】傾斜角を示す図である。
【技術分野】
【０００１】
本発明はロボット等の運動体に設けられる加速度センサの出力値を補正する技術に関する。
【背景技術】
【０００２】
ロボット等の移動体の姿勢制御に加速度センサやヨーレートセンサが用いられている。直交する３軸をｘ軸、ｙ軸、ｚ軸とすると、各軸方向の加速度を３個の加速度センサで検出し、各軸回りのヨーレートを３個のヨーレートセンサで検出する。軸回りの角度、あるいは姿勢角（ピッチ角、ロール角、ヨー角）は、ヨーレートセンサの出力を時間積分して得られる。
【０００３】
特開２００４−２６８７３０号公報には、ジャイロセンサから出力される加速度データ及び姿勢データを用いて姿勢制御する技術が開示されている。
【０００４】
加速度センサには零点オフセットが存在し、運動体の静止時に零点オフセットを補正することが必要であるが、静止時においても重力加速度があるため、零点を決定することができない。もちろん、零点安定性や精度の高い加速度センサを用いればよいが、高価でサイズや重量も大きくなる。
【発明の開示】
【発明が解決しようとする課題】
【０００５】
本発明の目的は、簡易な構成で加速度センサの出力値を補正し、高精度に加速度、ひいては運動体の姿勢角を検出できる技術を提供することにある。
【課題を解決するための手段】
【０００６】
本発明の第１の態様は、運動体に設けられた加速度センサからの出力値に基づき運動体の姿勢角データを演算する手段と、前記姿勢角データと基準姿勢角データとを比較することで前記加速度センサの出力値を補正する手段とを有する加速度センサの補正装置に関する。
【０００７】
この補正装置によれば、加速度センサの出力値からロボット等の運動体の姿勢角データが演算される。この姿勢角データを加速度センサによる検出とは別個に検出され、あるいは設定された基準姿勢角と前記演算された姿勢角データとが比較される。加速度センサの出力値に零点オフセットあるいは感度異常が存在する場合、出力値に基づき演算された姿勢角は、基準姿勢角データと異なる値を示す。そこで、両姿勢角データを比較することで、加速度センサの出力値の異常、及びその度合いを検出して補正することができる。上記補正装置では、加速度センサで検出された加速度自体ではなく、加速度から得られる姿勢角データ同士を比較するため、重力加速度の影響によらず高精度に補正できる。
【０００８】
本発明によれば、簡易な構成で加速度センサの出力値を補正し、高精度に加速度や運動体の姿勢角を検出できる。
【０００９】
本発明の第２の態様は、加速度センサの出力値を補正する方法に関する。この方法は、加速度センサからの出力値に基づき運動体の姿勢角データを演算するステップと、前記姿勢角データと基準姿勢角データとを比較するステップと、前記姿勢角データと基準姿勢角データとの比較結果に基づき、前記加速度センサの出力値を補正するステップとを有する。
【発明を実施するための最良の形態】
【００１０】
以下、図面に基づき本発明の実施形態について説明する。
【００１１】
＜第１実施形態＞
図１に、本実施形態の構成ブロック図を示す。加速度センサ１０は、ロボット等の運動体の所定位置に所定の姿勢で設けられ、運動体の加速度を検出して補正演算器１２に出力する。
【００１２】
補正演算器１２は、後述する零点補正器２６及び感度補正器２８からの補正データに基づいて加速度センサ１０の出力値を補正し、出力器２４に出力する。また、補正演算器１２は、補正した出力値を姿勢角演算器１４に出力する。
【００１３】
姿勢角演算器１４は、補正演算器１２からの出力値に基づき傾斜角を演算し、傾斜角に基づき姿勢行列を演算し、この姿勢行列に基づき運動体の姿勢角を演算する。加速度から傾斜角の演算、及び傾斜角から姿勢角の演算については後述する。姿勢角演算器１４は、演算して得られた姿勢角を姿勢角比較器１６に出力する。
【００１４】
姿勢角比較器１６は、出力値から得られた姿勢角（加速度姿勢角）と、レジスタ２０に設定された姿勢角（基準姿勢角）とを比較し、その差が所定の許容値以上か否かを判定する。加速度姿勢角と基準姿勢角との差が所定の許容値以上である場合には、出力値を補正する必要があると判断して、加速度姿勢角と基準姿勢角との差を補正値演算器１８に出力する。
【００１５】
補正値演算器１８は、入力した差を用いて出力値の零点及び感度を補正するために必要な補正値を演算し、それぞれ零点補正器２６及び感度補正器２８に出力する。零点補正器２６は、補正演算器１２に対して零点補正に必要な零点オフセット値を補正演算器１２に出力する。補正演算器１２は、出力値から零点オフセットを除去することで出力値を補正する。また、感度補正器２８は、補正演算器１２に対して感度補正に必要な係数（ゲイン）を補正演算器１２に出力する。出力値の補正は、零点補正器２６による零点補正のみでもよい。
【００１６】
加速度姿勢角と比較されるべき基準姿勢角は、上記のようにレジスタ２０に設定される。レジスタ２０に設定される基準姿勢角は、予めロボットを特定の姿勢に維持したときの姿勢角であるが、精度が確保されている限り、加速度センサ１０とは別個にロボットに設けられた姿勢角センサから入力器２２を介して供給してもよい。予め定めた姿勢において加速度姿勢角と基準姿勢角とを比較する場合、レジスタ２０に固定値を設定しておけばよく、入力器２２は必須でない。別個の姿勢角センサは、例えば光ファイバジャイロ（ＦＯＧ）等を用いることができる。光ファイバジャイロで得られた角速度は時間積分されて姿勢角が検出され、この姿勢角が入力器２２に供給されてレジスタ２０に設定される。加速度センサ１０が鉛直方向の加速度を検出し、運動体であるロボットが直立して静止している場合、直立時の基準姿勢角がレジスタ２０に設定され、加速度姿勢角と比較される。加速度センサ１０が正確に１Ｇを出力していれば加速度姿勢角と基準姿勢角とが所定の許容値の範囲内で一致するが、そうではない場合、その差に応じて加速度センサ１０の出力値を補正する。ロボットが傾いている場合、鉛直軸以外にも加速度成分が生じるが、そのときの加速度姿勢角と基準姿勢角とを比較することで、加速度センサ１０の出力値を補正できる。
【００１７】
姿勢角比較器１６では、加速度姿勢角と基準姿勢角を比較しているが、姿勢角演算器１４で演算された傾斜角をレジスタ２０に設定された基準姿勢角と比較してもよく、あるいは姿勢角演算器１４で演算された姿勢行列をレジスタ２０に設定された基準姿勢行列と比較してもよい。さらに、姿勢角演算器１４で姿勢角の４元数を演算し、この４元数をレジスタ２０に設定された基準４元数と比較してもよい。姿勢角、傾斜角、姿勢行列、４元数を本実施形態では「姿勢データ」と総称する。
【００１８】
以下、加速度から傾斜角を演算する方法、傾斜角から姿勢行列を演算する方法、姿勢行列から姿勢角を演算する方法について説明する。
【００１９】
まず、姿勢行列について説明する。基準座標系ＸＹＺにおけるセンサ座標系の表記法として、離散時間ｎにおける姿勢行列Ｔ（ｎ）で表す。姿勢行列Ｔ（ｎ）は、（１）式に示すように４×４の要素から構成される。
【００２０】
【数１】
【００２１】
行列Ｔ（ｎ）の意味として、第一列（ａ，ｂ，ｃ）、第二列（ｄ，ｅ，ｆ）、第三列（ｇ，ｈ，ｉ）は、それぞれ基準座標系からみたセンサ座標系ｎのｘ軸、ｙ軸、ｚ軸の方向ベクトルを表す。第四列は、基準座標系におけるセンサ座標系ｎの原点位置を表す（一般に並進が有る場合はこの第四列に並進量が表される）。原点の移動がない場合は、位置の変換を表す第４列の１〜３行目の要素を０とおく。図５に示すように、センサ座標系ｎの原点Ｏｎが、基準座標系において（０，０，０）の位置にあり、ｘ軸ベクトルは基準座標系上の（ａ，ｂ，ｃ）、ｙ軸ベクトルは（ｄ，ｅ，ｆ）、ｚ軸ベクトルは（ｇ，ｈ，ｉ）の成分を持つ。
【００２２】
姿勢角（ロール・ピッチ・ヨー角）から姿勢行列Ｔ（ｎ）を求める手法を以下に説明する。姿勢行列Ｔ（ｎ）を表すために行列による回転変換は、回転軸についての順序を考慮する必要がある。図６に示すように、ロボットで一般的に用いられるロール・ピッチ・ヨー角を用いる場合は、最初にｘ軸周りの回転φ、次に回転後のｙ軸周りの回転θ、最後に回転後のｚ軸周りの回転ψの３回の回転が生じたと定義する（軸の回転順番が固定されている点に注意）。
【００２３】
ロール・ピッチ・ヨー角による変換行列をＲＰＹ（φ、θ、ψ）とする。ＲＰＹ（φ、θ、ψ）は回転変換行列を左から右に掛けた行列の積となり、式（２）で表される。
【数２】
【００２４】
式（２）は具体的に式（３）で表される。
【数３】
【００２５】
式（３）を書き下すと、式（４）で表記できる。
【数４】
【００２６】
なお、ロール・ピッチ・ヨー角の代わりに、オイラー角を姿勢角として用いることもできる。オイラー角では、最初にｘ軸周りの回転φ、次に回転後のｙ軸周りの回転θ、最後に回転後のｚ軸周りの回転ψが起こった時の変換行列をＥｕｌｅｒ（Ｅφ、Ｅθ、Ｅψ）とおき、式（５）で表される。
【数５】
【００２７】
式（５）は具体的に式（６）で表される。
【００２８】
【数６】
【００２９】
式（６）を書き下すと、式（７）で表記できる。
【００３０】
【数７】
【００３１】
基準座標系をＯ−ＸＹＺ、初期のセンサ座標系をＯ0−ｘ0ｙ0ｚ0とおく。基準座標系と時刻ｔ＝０時の座標系Ｏ0−ｘ0ｙ0ｚ0の関係を、座標変換Ａ（０）で関係づける。時刻ｔ＝ｔn時の座標系をＯn−ｘnｙnｚnとおく。各座標系の原点Ｏ，Ｏ0，Ｏnは位置の移動がなく同一とする。その後運動体の姿勢変化により、図７に示すように、座標系Ｏ(n-1)−ｘ(n-1)ｙ(nー１)ｚ(n-1)からＯn−ｘnｙnｚnへ変化したとき、Ｏ(n-1)−ｘ(n-1)ｙ(nー１)ｚ(n-1)とＯn−ｘnｙnｚnは出力値から求まる行列Ａ（ｎ）で関係づけられる。基準座標系からみたセンサ座標系Ｔ（ｎ）は、変換Ａ（ｎ）を右からかけていくことで式（８）で求める。センサ座標系の原点が時間と共に移動する場合は、行列Ａの第４列の１〜３行目の要素に時間と共に移動した座標が逐次入れられる。行列Ａの第４列はセンサ座標系の回転に対しては影響を与えないので、ここでは特に述べない。
【００３２】
【数８】
【００３３】
次に、光ファイバジャイロ等の角速度出力値から微小回転行列Ａ（ｎ）行列を導出する方法について説明する。３個の角速度センサは、センサ座標系の各軸に設置されており、図８に示すように、センサｘ，ｙ，ｚ軸まわりの角速度を計測している。式（４）において、回転角Δφ，Δθ，Δψが十分小さいとき、
【数９】
【数１０】
である。これよりセンサｘ軸回りの微小回転角Δφ、センサｙ軸回りの微小回転角Δθ、センサｚ軸回りの微小回転角Δψを用いて式（１１）で表すことができる。式（１１）の結果は行列の各要素が独立に各微小回転角で表せるため、回転の順番に依存しないと近似している。
【００３４】
【数１１】
【００３５】
微小角と出力値との間には、微小回転角Δφ、Δθ、Δψ、角速度センサからの出力値ωｘ，ωｙ，ωｚ，サンプリング周期ｔｓから、式 （１２）〜（１４）の関係がある。サンプリング周期ｔｓが回転運動に対して充分早い周期としているため、サンプリング周期ｔｓの時間内での回転は充分小さく、微小回転角とみなすことができる。
【００３６】
【数１２】
【数１３】
【数１４】
【００３７】
このためＡ（ｎ）行列は式（１５）で表される。
【数１５】
【００３８】
次に、姿勢行列から姿勢角を求める手法について述べる。
【００３９】
姿勢行列Ｔ（ｎ）が式（１６）で表されている。
【数１６】
【００４０】
ヨー角ψは、
【数１７】
姿勢角であるヨー角ψの変域は、−π＜ψ≦πである。
【００４１】
ロール角φは、
【数１８】
姿勢角であるロール角φの変域は、−π／２≦φ≦π／２である。
【００４２】
ピッチ角θは、
【数１９】
姿勢角であるピッチ角θの変域は、 −π＜θ≦πである。
【００４３】
オイラー角を用いる場合は、式（２０）〜（２３）を使う。
【数２０】
【数２１】
【数２２】
【数２３】
【００４４】
次に、行列の正規化について説明する。
【数２４】
【００４５】
姿勢行列Ｔ（ｎ）では、演算後、姿勢行列の各列が単位ベクトルとならないことがあるため式（２４）の各列ベクトルの大きさが１になるように式（２５）で正規化を行う。
【００４６】
【数２５】
【００４７】
ここでｐ1，ｐ2は式（２６）、（２７）で与えられる。
【数２６】
【数２７】
【００４８】
その後、正規化後の各要素をあらためて、
【数２８】
と置き直す。
【００４９】
さらに、行列の直交化について説明する。
【数２９】
【００５０】
姿勢行列Ｔ（ｎ）では演算後、姿勢行列の各列が直交した軸とならないことがあるため、式（２９）の各列ベクトルが直交する直交化処理を行う（この場合、ｚ軸を基準としている）。ｚ軸、ｙ軸に直交する新しいｘ’軸を得るため、ａ’，ｂ’，ｃ’を求める。
【００５１】
【数３０】
【数３１】
【数３２】
【００５２】
次に、ｚ軸、ｘ’軸に直交する新しいｙ’軸を得るために、ｄ’，ｅ’，ｆ’を求める。
【数３３】
【数３４】
【数３５】
【００５３】
求めたａ’〜ｆ’から、直交化した姿勢行列Ｔ（ｎ）を得る。
【数３６】
【００５４】
ここで、ａｔａｎ２について説明する。ａｔａｎ２（ｙ，ｘ）は、２変数ｘ，ｙを持つ計算機用関数である。通常使われるａｔａｎ関数よりも適用範囲が広い。
【数３７】
【００５５】
（−π＜ξ≦π）
は、
ｘ＞０、ｙ＞０の時
【数３８】
となり、
ｘ＞０、ｙ＜０の時
【数３９】
となる。同様にして、
ｘ＜０，ｙ＞０の時
ξ＝π＋ｔａｎ-1（ｙ／ｘ）
となり、
ｘ＜０，ｙ＜０の時
ξ＝−π＋ｔａｎ-1（ｙ／ｘ）
となり、
ｘ＝０，ｙ＞０の時
ξ＝π／２
となり、
ｘ＝０，ｙ＜０の時
ξ＝−π／２
となり、
ｘ＝０，ｙ＝０の時
ξ＝０
となる。
【００５６】
次に、傾斜角の演算について説明する。加速度センサ１０からの加速度に基づき、姿勢角演算器１４で傾斜角を演算する方法である。傾斜角とは、センサｘ、ｙ、ｚ軸と基準Ｚ軸との間の角度λｘ、λｙ、λｚである。すなわち、
λｘ：ｘ軸とＺ軸の間の角度
λｙ：ｙ軸とＺ軸の間の角度
λｚ：ｚ軸とＺ軸の間の角度
であり、λｘ、λｙ、λｚの範囲は、０≦（λｘ、λｙ、λｚ）≦πである。図９に、傾斜角と重力ベクトルを示す。センサ座標に配置された加速度センサから以下のように傾斜角を求める。加速度Ｇｘ、Ｇｙ、Ｇｚを式（４０）〜（４２）用いて正規化し、正規化後の加速度Ｇｘ’、Ｇｙ’、Ｇｚ’を求める。
【００５７】
【数４０】
【数４１】
【数４２】
【００５８】
加速度Ｇｘ’、Ｇｙ’、Ｇｚ’から、式（４３）〜（４５）を用いて傾斜角λｘ、λｙ、λｚを求める。
【００５９】
【数４３】
【数４４】
【数４５】
【００６０】
次に、傾斜角λｘ、λｙ、λｚ から姿勢行列Ｔ（ｎ）を求める手法について述べる。姿勢角演算器１４で傾斜角に基づき姿勢行列を求める演算である。
【００６１】
【数４６】
【数４７】
【数４８】
【数４９】
【数５０】
【数５１】
【数５２】
【数５３】
【数５４】
【数５５】
【数５６】
【００６２】
以上の結果から、姿勢行列Ｔ（ｎ）を求める。
【００６３】
なお、姿勢行列Ｔ（ｎ）から傾斜角λｘ、λｙ、λｚを求める際には以下の式を用いる。
【数５７】
【数５８】
【数５９】
【数６０】
【００６４】
このように、本実施形態では、加速度センサ１０で得られた姿勢角と、別個の姿勢角センサで得られた基準姿勢角とを比較することで、加速度センサの出力値を簡易に補正することができる。
【００６５】
＜第２実施形態＞
図２に、本実施形態の構成ブロック図を示す。図１と異なる点は、加速度センサ１０として加速度センサ１０ａ、１０ｂ、１０ｃが３個設けられてｘ、ｙ、ｚ各軸方向の加速度を検出し、また、加速度センサ１０ａ、１０ｂ、１０ｃに対応して補正演算器１２ａ、１２ｂ、１２ｃが設けられる点である。
【００６６】
３個の加速度センサ１０ａ、１０ｂ、１０ｃにより加速度を検出し、これらの出力値から姿勢角を演算することで、運動体の姿勢を一義的に特定することができる。運動体の姿勢を順次変化させて特定の姿勢を実現し、これら特定の姿勢において検出された姿勢角をレジスタ２０に設定された基準姿勢角と比較する。例えば、ロボットの姿勢を順次変化させてｘ軸、ｙ軸、ｚ軸が順次Ｚ軸方向（鉛直方向）を向くようにし、そのときの加速度姿勢角と基準姿勢角との差を用いて各加速度センサ１０ａ、１０ｂ、１０ｃの出力値を順次補正する。加速度センサ１０ａ、１０ｂのみでもよく、一般に複数ｎ個（ｎ≧２）の加速度センサを設けることができる。
【００６７】
なお、図では便宜上、零点補正器２６からの補正信号は補正演算器１２ａのみに出力されているが、他の補正演算器１２ｂ、１２ｃにも補正信号が出力されてもよい。感度補正器２８についても同様である。
【００６８】
＜第３実施形態＞
図３に、本実施形態の構成を示す。上記の各実施形態において、出力値の補正はロボットが特定の姿勢で静止している場合に実行される。したがって、例えば、ユーザあるいはロボットのメインプロセッサからの指示により加速度センサの補正装置が補正を実行する構成の場合、補正実行命令を受けたときにロボットが静止しており補正を実行できるタイミングであるか否かを判定する必要がある。図３における静止判定器３０は、外部からの補正実行命令を受けてロボットが静止状態にあるか否かを判定する。
【００６９】
静止判定器３０は、姿勢角演算器１４からの姿勢角の変化量を検出し、この変化量が所定値以下であるか否かを判定する。姿勢角の変化量が所定値以下である場合にはロボットは静止状態にあると判定し、補正値演算器１８に補正許可信号を出力する。補正値演算器１８は、補正許可信号を受信することで補正値を演算し、零点補正器２６等に出力する。静止判定器３０は、姿勢角の変化量ではなく、加速度センサ１０からの出力値自体の変化量を検出し、この変化量を所定値と比較して静止状態を判定してもよい。ロボットが静止しておらず運動している場合、並進加速度及び遠心加速度が重畳し、かつ、補正されるべき出力値が時間とともに変化するため補正精度が著しく低下する。ロボットの静止状態において出力値の補正を実行することで、補正精度を確保できる。
【００７０】
図４に、本実施形態の処理フローチャートを示す。まず、ユーザ（あるいはユーザの指示を受けたメインプロセッサ）からの補正命令を入力し、姿勢角として、ピッチ角θｉ、ロール角φｉ、及びヨー角yｉを入力する（Ｓ１０１）。ユーザ（あるいはユーザの指示を受けたメインプロセッサ）から入力された基準姿勢角（φｉ、θｉ、yｉ）はレジスタ２０に設定される。静止判定器３０は、この補正命令を受信すると、加速度センサ１０ａ、１０ｂ、１０ｃからの出力値、あるいは姿勢角演算器１２ａ、１２ｂ、１２ｃからの姿勢角の変化量（時間変動幅）を検出し、所定値以下であるか否かを判定する（Ｓ１０２）。変化量が所定値以下である場合、静止判定器３０はロボットが静止状態にあると判定する。なお、変化量が所定値以下である時間を所定のしきい時間と比較し、所定のしきい時間以上である場合のみロボットが静止状態にあると判定してもよい。所定のしきい時間は例えば３秒に設定でき、これにより補正に必要な有意の静止状態を検出できる。
【００７１】
静止判定器３０でロボットが静止状態にあると判定した場合、静止判定器３０は上記のように補正許可信号を補正値演算器１８に出力する。補正値演算器１８は、この補正許可信号により、そのときの加速度姿勢角と基準姿勢角との差に基づき、この差が減少又はなくなるように補正値を演算して出力する。補正演算器１２ａ、１２ｂ、１２ｃは補正値を用いて出力値の零点補正あるいは感度補正を行う（Ｓ１０３）。
【００７２】
次に、補正を繰り返し行うか否かを判定し（Ｓ１０４）、複数回行う必要があればロボットの姿勢を変化させ（Ｓ１０５）、新たに基準姿勢角（φｊ、θｊ、yｊ）を入力して同様の補正処理を行う。３個の加速度センサ１０ａ、１０ｂ、１０ｃの全てについて補正することが好適であり、この場合に少なくとも３回補正処理を繰り返す。例えば、（０、０、０）としてｚ軸方向の加速度センサ１０ｃについて感度補正を行い、次に（π／２、０、０）としてｘ軸方向の加速度センサ１０ａについて感度補正を行い、次に（０、π／２、０）としてｙ軸方向の加速度センサ１０ｂについて感度補正を行うである。補正を実行する姿勢としては、加速度センサ１０ａ、１０ｂ、１０ｃの検出方向が基準座標系の軸に対して平行となる姿勢が好適であるが、必ずしも平行である必要はない。３個の加速度センサにおいて、ｘ軸方向、ｙ軸方向の２個の加速度センサ１０ａ、１０ｂについて補正を行い、ｚ軸方向は補正を行わずとも精度が得られる場合は、２回の補正を行えばよい。これは、ロボットの姿勢が大きく傾かない場合などが想定される。
【００７３】
上記の実施形態において、コントローラは汎用プロセッサで実現される。コントローラは、全体の制御のためのメインあるいは中央処理部、及び種々の異なる特定の計算や機能を中央処理部の制御の下に実行する独立した機能部を有する特定用途向けのＩＣ（例えばＡＳＩＣ）で実現され得ることは当業者には理解されよう。 コントローラは、専用のあるいはプログラム可能な複数の集積回路あるいはその他の電子回路や電子装置（例えば、ワイヤ接続された電子あるいは論理回路、ＰＬＤ、ＰＬＡ、ＰＡＬ等のプログラマブルロジックデバイス）でもよい。コントローラは、マイクロプロセッサ、マイクロコントローラ、あるいは他の処理装置（ＣＰＵ、ＭＰＵ）等の汎用コンピュータで使用するようにプログラムされ得る。実施形態で述べた手順を実行できる任意の装置あるいは装置群がコントローラとして使用可能である。データ／信号処理能力の最大化と高速化のために分散処理構造を用いることもできる。
【００７４】
本発明の実施形態について説明したが、本発明は上記の実施形態に限定されるものではない。本発明は種々の変形例や均等物を含む。種々の実施形態が示されているが、本発明はこれらの組み合わせも含む。
【図面の簡単な説明】
【００７５】
【図１】実施形態の構成ブロック図である。
【図２】他の実施形態の構成ブロック図である。
【図３】さらに他の実施形態の構成ブロック図である。
【図４】実施形態の補正処理フローチャートである。
【図５】基準座標系（ＸＹＺ）とセンサ座標系（ｘｙｚ）との関係を示す図である。
【図６】基準座標系における姿勢角（ロール角、ピッチ角、ヨー角）を示す図である。
【図７】センサ座標系ｎの時間的変化を示す図である。
【図８】センサ座標系における微小回転角を示す図である。
【図９】傾斜角を示す図である。
【特許請求の範囲】
【請求項１】
加速度センサの補正装置であって、
運動体に設けられた加速度センサからの出力値に基づき運動体の姿勢角データを演算する演算手段と、
前記姿勢角データと基準姿勢角データとを比較することで前記加速度センサの出力値を補正する補正手段と、
を有する加速度センサの補正装置。
【請求項２】
請求項１記載の装置において、さらに、
前記基準姿勢角を前記運動体の特定姿勢における姿勢角として設定する設定手段
を有する加速度センサの補正装置。
【請求項３】
請求項１、２のいずれかに記載の装置において、
前記加速度センサは複数ｎ個（ｎ≧２）設けられ、
前記補正手段は、前記運動体の異なるｎ個の特定姿勢において出力値を補正する加速度センサの補正装置。
【請求項４】
請求項１〜３のいずれかに記載の装置において、さらに、
前記運動体の静止状態を検出する検出手段
を有し、前記補正手段は、前記静止状態において前記出力値を補正する加速度センサの補正装置。
【請求項５】
請求項４記載の装置において、
前記検出手段は、前記加速度センサの出力値の変化量が所定値以下であるか否かにより 前記静止状態を検出する加速度センサの補正装置。
【請求項６】
請求項４記載の装置において、
前記検出手段は、前記演算手段からの姿勢角データの変化量が所定値以下であるか否かにより前記静止状態を検出する加速度センサの補正装置。
【請求項７】
請求項４〜６のいずれかに記載の装置において、さらに、
前記出力値を補正するための補正命令信号を入力する入力手段
を有し、前記検出手段は、前記補正命令信号を入力した場合に前記静止状態を検出する加速度センサの補正装置。
【請求項８】
請求項１〜７のいずれかに記載の装置において、
前記補正手段は、前記出力値の零点あるいは感度の少なくともいずれかを補正する加速度センサの補正装置。
【請求項９】
加速度センサの出力値を補正する方法であって、
加速度センサからの出力値に基づき運動体の姿勢角データを演算し、
前記姿勢角データと基準姿勢角データとを比較し、
前記姿勢角データと基準姿勢角データとの比較結果に基づき、前記加速度センサの出力値を補正する方法。
【請求項１０】
加速度センサの補正装置であって、
運動体に設けられた加速度センサからの出力値に基づき運動体の姿勢角データを演算する演算装置と、
前記姿勢角データと基準姿勢角データとを比較することで前記加速度センサの出力値を補正する補正装置と、
を有する加速度センサの補正装置。
【請求項１】
加速度センサの補正装置であって、
運動体に設けられた加速度センサからの出力値に基づき運動体の姿勢角データを演算する演算手段と、
前記姿勢角データと基準姿勢角データとを比較することで前記加速度センサの出力値を補正する補正手段と、
を有する加速度センサの補正装置。
【請求項２】
請求項１記載の装置において、さらに、
前記基準姿勢角を前記運動体の特定姿勢における姿勢角として設定する設定手段
を有する加速度センサの補正装置。
【請求項３】
請求項１、２のいずれかに記載の装置において、
前記加速度センサは複数ｎ個（ｎ≧２）設けられ、
前記補正手段は、前記運動体の異なるｎ個の特定姿勢において出力値を補正する加速度センサの補正装置。
【請求項４】
請求項１〜３のいずれかに記載の装置において、さらに、
前記運動体の静止状態を検出する検出手段
を有し、前記補正手段は、前記静止状態において前記出力値を補正する加速度センサの補正装置。
【請求項５】
請求項４記載の装置において、
前記検出手段は、前記加速度センサの出力値の変化量が所定値以下であるか否かにより 前記静止状態を検出する加速度センサの補正装置。
【請求項６】
請求項４記載の装置において、
前記検出手段は、前記演算手段からの姿勢角データの変化量が所定値以下であるか否かにより前記静止状態を検出する加速度センサの補正装置。
【請求項７】
請求項４〜６のいずれかに記載の装置において、さらに、
前記出力値を補正するための補正命令信号を入力する入力手段
を有し、前記検出手段は、前記補正命令信号を入力した場合に前記静止状態を検出する加速度センサの補正装置。
【請求項８】
請求項１〜７のいずれかに記載の装置において、
前記補正手段は、前記出力値の零点あるいは感度の少なくともいずれかを補正する加速度センサの補正装置。
【請求項９】
加速度センサの出力値を補正する方法であって、
加速度センサからの出力値に基づき運動体の姿勢角データを演算し、
前記姿勢角データと基準姿勢角データとを比較し、
前記姿勢角データと基準姿勢角データとの比較結果に基づき、前記加速度センサの出力値を補正する方法。
【請求項１０】
加速度センサの補正装置であって、
運動体に設けられた加速度センサからの出力値に基づき運動体の姿勢角データを演算する演算装置と、
前記姿勢角データと基準姿勢角データとを比較することで前記加速度センサの出力値を補正する補正装置と、
を有する加速度センサの補正装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【公表番号】特表２００９−５０３５３０（Ｐ２００９−５０３５３０Ａ）
【公表日】平成２１年１月２９日（２００９．１．２９）
【国際特許分類】
【出願番号】特願２００８−５２４６１０（Ｐ２００８−５２４６１０）
【出願日】平成１８年８月１日（２００６．８．１）
【国際出願番号】ＰＣＴ／ＩＢ２００６／００２０８８
【国際公開番号】ＷＯ２００７／０１５１３８
【国際公開日】平成１９年２月８日（２００７．２．８）
【新規性喪失の例外の表示】特許法第３０条第３項適用申請有り
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
【公表日】平成２１年１月２９日（２００９．１．２９）
【国際特許分類】
【出願日】平成１８年８月１日（２００６．８．１）
【国際出願番号】ＰＣＴ／ＩＢ２００６／００２０８８
【国際公開番号】ＷＯ２００７／０１５１３８
【国際公開日】平成１９年２月８日（２００７．２．８）
【新規性喪失の例外の表示】特許法第３０条第３項適用申請有り
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
[ Back to top ]