JP3559965B2

JP3559965B2 - 姿勢角検出装置

Info

Publication number: JP3559965B2
Application number: JP2001199151A
Authority: JP
Inventors: 靖裕佐藤; 美行芳賀
Original assignee: Japan Aviation Electronics Industry Ltd
Current assignee: Japan Aviation Electronics Industry Ltd
Priority date: 2001-06-29
Filing date: 2001-06-29
Publication date: 2004-09-02
Anticipated expiration: 2021-06-29
Also published as: JP2003014460A

Description

【０００１】
【発明の属する技術分野】
この発明はジャイロと加速度計とを搭載して姿勢角を検出する装置に関する。
【０００２】
【従来の技術】
例えばこの種の姿勢角検出装置を人間の頭部に取付け、その頭部の動きを検出して、その頭部の動きに応じて表示器に表示される仮想空間画像が変化するようにされたものがある。
従来の姿勢角検出装置は図７に示すように、例えば互いに直交する３軸を入力軸とするジャイロ１１の各出力はＡＤ変換器１２でデジタル値に変換され、これらデジタル値は減算器１３でリセット時の初期バイアスが減算され、更に補正部１４で補正されてボディ角速度ω_Ｂとされる。
【０００３】
一方、直交する軸を入力軸とする加速度計１５の出力は、その出力がアナログ信号の場合はＡＤ変換器１６でデジタル値に変換され、更に補正部１７で補正されてボディ加速度Ａ_Ｂとして出力される。
姿勢基準計算部１８よりの方向余弦マトリクスＣとボディ加速度Ａ_Ｂが水平加速度計算部１９に入力されて、水平加速度Ａ_Ｌが計算され、その水平加速度Ａ_Ｌは水平加速度ダンピング部２１で角速度誤差Δω_ＢＡが推定され、この角速度誤差Δω_ＢＡがボディ角速度ω_Ｂから引算器２２で引算され、その引算出力は姿勢基準計算部１８で角速度積分されて、姿勢基準が計算され、この姿勢基準はオイラー角計算部２３でオイラー角に変換され、その局所座標系の姿勢角として出力される。
【０００４】
補正部１４及び１７はＡＤ変換後のフィルタ処理や、センサの製品出荷時に行われる調整時に取得されるミスアライメント補正値、スケールファクタ補正、バイアス補正値などによる補正が行われる。
水平加速度ダンピング部２１では、図８に示すように方向余弦マトリクスＣとボディ加速度Ａ_Bが水平加速度計算部１９で乗算されて水平加速度Ａ_Lが求められ、水平加速度Ａ_Lから重力加速度（０，０，Ｇ）が引算器２５で引算され、水平成分ΔＡ_L＝（ΔＡ_{L X}，ΔＡ_{L Y}，０）が加速度計算部２６で角速度Δω_L1＝（Δω_L1X，Δω_L1Y，０）が計算され、Δω_L1は座標変換部２７で方向余弦マトリクスＣによりボディ座標成分Δω_B1に変換される。またΔＡ_Lは加速度計算部２８で角速度Δω_L2＝（Δω_L2X，Δω_L2Y，０）が計算され、Δω_L2は座標変換部２９でボディ座標成分に変換された後積分器３１で積分される。この積分結果とΔω_B1が加算器３２で加算されて角速度誤差Δω_BAとされる。
【０００５】
水平加速度ダンピング計算部２１は、図９に示すように構成されることもある。図９において、図８と異なる部分は水平加速度成分ΔＡ_Lは角度変換部３３で誤差角度成分（Δφ，Δθ，０）が計算され、この誤差角度成分は姿勢基準計算部１８の出力に対し、引算器３４で姿勢基準の回転（トルキング）を行うことにより、誤差補正を行う。
人間の頭部に姿勢角検出装置を取付ける場合はその大きさを数ｃｍ立方、重さを１００ｇ以下程度の小形軽量なものとする必要がある。この点から使用されるジャイロ（角速度センサ）や加速度計は低精度でしかも誤差のばらつきが大きいものを使用せざるを得ない。このような低精度のジャイロを使用した場合、角速度誤差が発生することにより、姿勢角がドリフトする現象が発生する。これを抑えるために、角度ドリフトがない傾斜計のような角度センサを用いて、ジャイロの誤差を推定するループを構成していた。
【０００６】
またリセット時、つまり初期ジャイロバイアス推定時に、ジャイロバイアス誤差が大きいジャイロについては角速度ゼロのＡＤ変換値がＡＤ変換器の中心から大きくずれて、ジャイロ出力の有効なＡＤ変換範囲が狭くなる。つまり従来においては図１０Ａに示すようにＡＤ変換器１２にジャイロ出力が直接入力され、ＡＤ変換器１２の出力は引算器１３を通じ、リセット時（ジャイロバイアス推定時）にオンにされるスイッチ３６を通じてジャイロバイアス推定部４２２に入力され、その値が初期バイアスとして保持されると共に引算器１３へ供給され、引算器１３の出力がゼロになるようにされている。
【０００７】
このようにジャイロセンサ出力がＡＤ変換器１２へ直接入力されているため、温度環境の異なる場所や、ジャイロバイアスやそのバイアスの変動による誤差の大きいセンサの場合には、例えば図１０Ｂに示すようにＡＤ変換器１２として入力レンジ（変換範囲）が０〜５Ｖであり、角速度がゼロの場合の入力が入力レンジの中心２．５Ｖのものが用いられているが、ジャイロバイアスやドリフト誤差が大きいため、角速度がゼロ時のジャイロ出力が３．２Ｖであったとすると、−側での入力変化に対応できる範囲（−側角速度レンジ）は３．２Ｖであるが、＋側角速度レンジは１．８Ｖしかない。このため有効レンジとしては±１．８Ｖ分しかとれず、つまり狭い方のレンジに制約されてしまう。この対策としては、性能の悪い角速度センサを事前に排除するか、調整用抵抗素子を取付けて出荷前に調整し、そのバイアス誤差値を図７の引算器１３に入力する必要があった。これらは何れも、高価なものとなり、しかも温度環境によりジャイロバイアスが大きく変動する場合には、対応できなかった。
【０００８】
例えば人間の頭部に姿勢角検出装置を取付けて使用する場合に、人間の頭部の鉛直軸に対して姿勢角検出装置の鉛直軸を正確に平行に取付けることは困難であり、期待しない軸回りに回転が入力されるおそれがある。つまり図１１Ａに示すように人間の頭部３８に姿勢角検出装置３９を取付けた場合に装置３９の水平軸を水平に、つまりロール角及びピッチ角が共に０度になるように取付けることができなかった場合、図１１Ａではピッチ方向にΔθ傾けて装置３９を取付けた場合には、真の鉛直軸回りの角速度ωが入力されると、装置３９の期待しない軸、つまりＸ軸にも角速度ωｓｉｎ（Δθ）が入力され、Ｚ軸にも角速度ωｃｏｓ（Δθ）が入力されてしまい、正しい姿勢角を検出できない。
【０００９】
【発明が解決しようとする課題】
姿勢角検出装置は、出荷前のジャイロバイアスの調整を不要とし、しかも大きなジャイロバイアスが入力されても、ＡＤ変換器のレンジを十分確保できるという第１の課題と、
角度ドリフトがない磁気方位センサのような角度センサを用いることなく、ジャイロ出力を利用して、方位角（ヨー角）のドリフトを抑圧することができるという第２の課題と、
【００１０】
被取付け体の鉛直軸に対して装置の鉛直軸が正確に平行に取付けられていなくても簡単に姿勢角を決定することができるという第３の課題とがあるが、本願発明は上記第１の課題を解決することを目的とする。
【００１１】
【課題を解決するための手段】
上記第１の課題を解決するこの発明によれば、第１及び第２補正器が設けられ、これらはリセットコマンドにより動作され、ジャイロ出力が第１補正器に直接入力され、推定されたジャイロバイアス誤差が引算され、その引算されたアナログ値から前記引算結果が小さくなるように上記ジャイロバイアス誤差が推定される。この第１補正器により補正されたジャイロ出力は、第２補正器において推定されたデジタル値のジャイロバイアス補正残差が引算され、その引算された結果のデジタル値が小さくなるようにジャイロバイアス補正残差が推定され、そのジャイロバイアス補正残差が引算されたデジタル値のジャイロ出力が姿勢角を求めるために利用される。
【００１２】
上記第２の課題を解決するために次の技術が考えられる。角速度バイアス推定部においてジャイロ出力から得られる角速度が所定値以下になると本装置への角速度入力がゼロの状態と判定し、水平加速度ダンピング計算部の入力がゼロとされ、ジャイロ出力から推定したジャイロバイアス誤差を引算し、その引算結果から前記ジャイロバイアス誤差が推定される。
上記第３の課題を解決するために次の技術が考えられる。リセットコマンドにより初期姿勢角計算部が動作し、加速度計の出力から初期姿勢角が計算され、姿勢角設定部において初期姿勢角により姿勢基準が補正され、その補正された姿勢基準がオイラー角計算に用いられる。
【００１３】
【発明の実施の形態】
以下図面を参照してこの発明の実施形態を実施例により説明する。図１はこの発明の実施例を示し、図７と対応する部分には同一参照番号を付けてある。この実施例ではジャイロバイアス補正部４０が設けられ、ジャイロ１１の出力からジャイロバイアス誤差が差し引かれる。ジャイロバイアス補正部４０においてはジャイロ１１の出力はまず第１補正器４１でリセット時の推定ジャイロバイアス誤差がアナログ演算により差し引かれて補正され、その補正されたジャイロ出力はＡＤ変換器１２によりデジタル値に変換され、そのデジタル値のジャイロ出力は第２補正器４２でリセット時の推定ジャイロバイアス補正残差がデジタル演算により差し引かれて補正される。この補正されたジャイロ出力は補正部１４で補正され、引算器２２へ供給される。
【００１４】
引算器２２の出力は角速度バイアス推定部４３において本装置への角速度入力がゼロの状態とみなされた時の引算器２２の出力によりジャイロバイアス誤差が推定され、その推定ジャイロバイアス誤差が、引算器２２で補正部１４で補正されたジャイロ出力から差し引かれて補正される。水平加速度ダンピング部２１の入力側は水平加速度計算部１９の出力側と０レジスタ４４側とにスイッチ４５により切替え接続されるようにされ、角速度バイアス推定部４３において本装置への角速度入力がゼロの状態とみなされた時は、スイッチ４５は０レジスタ４４側に切替えられる。
【００１５】
更にこの例では姿勢基準計算部１８の入力側は引算器２２の出力側と、０度レジスタ４６とにスイッチ４７により切替え接続されるようになされた場合である。
初期姿勢角計算部４８においてリセット時の初期姿勢角が加速度計出力により計算され、その初期姿勢角分だけ、姿勢基準計算部１８よりの方向余弦マトリクスが出力姿勢角設定部４９で回転補正され、その回転補正された方向余弦マトリクスがオイラー角計算部２３へ供給される。
【００１６】
次にこの実施例において新たに設けた各部の具体例を説明する。
先ずリセット時のバイアス補正を行う第１及び第２補正器４１及び４２の例を図２Ａを参照して説明する。これら第１及び第２補正器４１及び４２はこの姿勢角検出装置を使用するごとにその始めにリセットスイッチ又は姿勢角検出装置を制御するパソコンからのリセットコマンドにより動作され、その時（本装置への角速度入力がゼロの状態）バイアス誤差値を求める。
第１補正器４１においてはジャイロ出力が減算器４１１に直接入力され、ＤＡ変換器４１２からのジャイロバイアス誤差Ｂ_ｉが引算され、その引算されたジャイロ出力がＡＤ変換器１２でデジタル値Δ_ｉに変換され、そのデジタル値は、リセットコマンドによりオンにされるスイッチ４１３を通じて誤差推定部４１４に入力される。誤差推定部４１４においては入力されたデジタル値はノイズ除去フィルタ４１５により雑音成分が除去され、誤差補正部４１６で前回のジャイロバイアス誤差Ｂ_ｉに対し、減算器４１１の出力Δ_ｉが加算されて、ジャイロバイアス誤差がＢ_ｉ＋１に更新され、この更新されたＢ_ｉ＋１がＤＡ変換器４１２によりアナログ信号に変換されて減算器４１１へ供給される。ＡＤ変換器１２の出力がゼロになるようにこの更新ループが何回か繰返され、ＡＤ変換器１２の出力値が、ＤＡ変換器４１２のＬＳＢ（最小桁ビット）以内になるまで行われる。
【００１７】
ノイズ除去フィルタ４１５は低域通過フィルタ又は移動平均を行うものなどが用いられる。最終的に得られたリセット時バイアス誤差は誤差補正部４１６内のレジスタに保持され、ＤＡ変換器４１２でアナログ信号とされて減算器４１１へ供給される。
第１補正器４１でリセット時のバイアス誤差Ｂ_ｉの値が確定した後、第１補正器４１の出力、この例ではＡＤ変換器１２の出力が第２補正器４２の減算器４２１へ供給され、補正残差推定部４２２からのジャイロバイアス補正残差が引算され、第１補正器４１における補正残差分（ＤＡ変換器４１２のＬＳＢ以下の値）がゼロになるように補正される。減算器４２１の出力はリセットコマンドによりオンにされるスイッチ４２３を通じて補正残差推定部４２２に入力される。補正残差推定部４２２では雑音を除去するために例えば約０．３秒間の平均値をとるフィルタ処理が行われて、第１補正器４１によるジャイロバイアス誤差の補正残差が推定され、これが引算器４２１へ供給される。この補正残差に対するジャイロバイアス補正値も例えばレジスタに保持され、減算器４２１へ供給され続ける。
【００１８】
ＡＤ変換器１２の変換レンジが０〜５．０Ｖの場合、図２Ｂに示すように静止時のジャイロ出力が３．２Ｖの場合は、＋角速度に対する変換レンジは１．８Ｖであり、−角速度に対する変換レンジは３．２Ｖであり、このままでは図１０Ｂで説明したように、有効角速度レンジは±１．８Ｖ分しかない。しかし、図２Ａ中のＤＡ変換器４１２からジャイロバイアス誤差として０．７Ｖが出力されると、静止時において減算器４１１の出力は２．５Ｖになり、図２Ｃに示すように、＋角速度レンジ及び−角速度レンジも共に２．５となり、有効角速度レンジが＋方向及び−方向において最大になる。なお、実際にはＤＡ変換器４１２は、そのＬＳＢ以内の変換誤差をもつため、減算器４１１の出力はＡＤ変換器１２の入力レンジ中央２．５Ｖに対しＤＡ変換器４１２のＬＳＢ以内の大きさの出力が減算器４１１から出力される。これが第２補正器４２により補正される。以上のようにして温度環境などによりジャイロバイアス誤差が大きく変化しても、使用の都度、リセット時のジャイロバイアス誤差値を求めることにより、角速度の検出範囲を常に最大に利用することができ、かつジャイロバイアス誤差を補正することができる。
【００１９】
図１中の姿勢基準計算部１８で姿勢角基準計算（積分）を行う点からＡＤ変換器１２の分解能を高くすることが望ましい。ＡＤ変換器１２の分解能を高くすると、その変換可能レンジが狭くなる。そのため図２Ａに示した構成では大きな初期ジャイロバイアスを取り除くことができなくなる。このような場合図３に示すような構成とすればよい。つまり減算器４１１の出力は、ＡＤ変換器１２よりも低分解能のＡＤ変換器４１７でデジタル値に変換し、その変換されたデジタル値がスイッチ４１３を通じてノイズ除去フィルタ４１５へ供給されるようにする。このようにしてレンジの広いＡＤ変換器４１７を用いて、大きなジャイロバイアス誤差に対しても、角速度がゼロ時のジャイロ出力が、減算器４１１で補正され、そのＡＤ変換器４１７の変換レンジのほぼ中心値になるようにされる。その後、減算器４１１の補正されたジャイロ出力は、ＡＤ変換器４１７よりも高分解能（例えば２０倍程度高い）のＡＤ変換器１２でデジタル値に変換され、第２補正器４２により、第１補正器４１での、リセット時のジャイロバイアス誤差の補正残差に対する補正値が求められる。図２Ａ及び図３に示したようにジャイロバイアス補正部４０では第１補正器４１によりまずアナログ量での補正を行っているため、大きなジャイロバイアス誤差があっても、デジタル値で行う場合にはＡＤ変換器の飽和のため、補正が不十分で有効動作範囲が狭まるおそれはない。なお、ＡＤ変換器１２，４１７などこの明細書でＡＤ変換器とは狭義のものではなく、例えば増幅器など他のものが含まれることがあり、要はアナログ値をデジタル値に変換するものであり、同様にＤＡ変換器４１２は狭義のものではなく、増幅器などを含んだものであってもよく、要はデジタル値をアナログ値に変換するものであればよい。
【００２０】
次に図１中の角速度バイアス推定部４３における角速度バイアス推定処理を、図４を参照して説明する。図１中の補正部１４からの補正されたジャイロ出力ω_Bはノイズ除去フィルタ４３１で、雑音成分が除去されて角速度誤差判定処理部４３２に入力される。ノイズ除去フィルタ４３１は移動平均処理や低域通過処理により雑音成分の除去を行う。このノイズ除去フィルタ４３１の特性はこの姿勢角検出装置の使用環境などにより決定する。
角速度誤差判定処理部４３２は、判定しきい値テーブル４３３を参照して入力された角速度が所定の角速度しきい値範囲内であれば誤差と判定して、スイッチ４３４及び４３５をオンにし、また図１中のスイッチ４５を０レジスタ４４側に切替え、必要に応じてスイッチ４７も０レジスタ４６側に切替える。ノイズ除去フィルタ４３１の誤差とみなされた出力はスイッチ４３４を通じて角速度バイアス推定処理部４３６に入力され、その時の角速度バイアス値Δω_iとされる。この角速度バイアス値Δω_iはスイッチ４３５を通じて角速度バイアス積分部４３７に入力され、それまでの角速度バイアスΔω_BS,i-1にΔω_iが加算されて角速度バイアス値Δω_BSが更新される。つまり下記の式が計算される。
【００２１】
Δω_BS,i＝Δω_BS,i-1＋Δω_i
この場合、図１中において水平加速度ダンピング部２１にスイッチ４５を通じて０が入力されているため図８及び図９における角速度誤差Δω_BAが変化するおそれがなく、正しく角速度バイアス値Δω_BSを求めることができる。
角速度誤差判定処理部４３２における誤差の判定は角速度しきい値範囲内であると連続してＮ回判定された時に、ノイズ除去フィルタ４３１の出力は本装置への角速度入力がゼロの状態に生じる誤差と判定するようにすることもできる。このためにはｊ＝０でＸ軸、ｊ＝１でＹ軸、ｊ＝２でＺ軸とし、１つのｊについての処理例を図５に示す。フラグＦ_j＝０とし（Ｓ１）ｊ軸入力のジャイロ出力（フィルタ４３１の出力）ω_jが、判定しきい値ω_jmの範囲、つまり−ω_jm＜ω_j＜＋ω_jmであるかを調べ（Ｓ２）、このしきい値範囲内になければｎ_jを０と（Ｓ３）、このしきい値範囲内にあればｎ_jを＋１して更新し（Ｓ４）、その更新されたｎ_jがＮ以上かを調べ（Ｓ５）、Ｎ以上であれば静止状態にあり、その時のω_jをｊ軸回り角速度誤差Δωと判定し、フラグＦ_jを１にする（Ｓ６）。図に示していないがｊを更新して同様に処理を行って次々と各軸回りの角速度の誤差を求める。
【００２２】
判定しきい値テーブル４３３に各ジャイロの入力軸Ｘ，Ｙ，Ｚごとに、各種値を格納しておき、利用者が判定しきい値を使用状態に応じて選ぶことにより入力角速度に対する感度を変更することができるようにしてもよい。判定しきい値を小さくすれば入力角速度に対する感度が高くなる。図示例では判定しきい値を全軸共通の２゜／秒としたが、各軸ごとに設定してもよい。図５に示したように各軸ごとにその入力がしきい値以内か否かを判定したが、全軸の入力ω_ｊが同時にしきい値以内とされた時のみ、その各軸のジャイロ出力を誤差と判定してもよい。場合によってはいずれかの軸の組合せについて行ってもよい。これらは姿勢角検出装置の利用の状況に応じて決めればよい。
【００２３】
角速度誤差判定処理部４３２において、その入力が判定しきい値の範囲外と判定されると、スイッチ４３４及び４３５はオフとされ、図１中のスイッチ４５及び４７に０レジスタ４４，４６から切り離されるように切替えられる。このスイッチ４３５がオフとされた時の角速度バイアス積分部４３７の出力が最新の角速度バイアスΔω_BSとして出力される。なお前記本装置への角速度入力がゼロの状態と判定され、スイッチ４３４，４３５がオフ、スイッチ４５，４７が０レジスタ４４，４６側に接続されている状態では図１において姿勢基準計算部１８の入力がゼロとなりオイラー角計算部２３はその時の姿勢角を出力し続けるため、例えばこの姿勢角に応じた画像を表示する場合、その画像が変動して見苦しくなるようなことはない。
【００２４】
また図４に示した角速度バイアスの推定処理によれば、各軸におけるジャイロ出力の角速度バイアスを推定できる。つまり、この姿勢角検出装置をほぼ水平面で使用した場合は、Ｚ軸ジャイロについては水平加速度ダンピングにより、補正がなされない。このため通常であれば使用した角速度センサの誤差によりヨー角がドリフトしてしまうが、図４に示した角速度バイアス推定処理によりヨー角のドリフトを抑えることができる。なお従来においては磁気方位センサなどジャイロ以外の方位センサを組み合せることによりヨー角の誤差補正を行っているが、この実施例によればそのようなジャイロ以外の方位センサを必要としない。
【００２５】
次に図１中の出力姿勢角設定部４９について説明する。図６に示すように加速度計出力が、リセット時にオンにされるスイッチ４９１を通じてノイズ除去フィルタ４９２に入力され、例えばＮ回の平均化が行われ、雑音除去されて加速度による姿勢角計算部４９３に入力される。この姿勢角計算部４９３においてその入力された加速度により姿勢角が計算され、この初期姿勢角が初期方向余弦マトリクスＣ_０として出力される。図１中の姿勢基準計算部１８において、その積分の初期値として、加速度から姿勢角を計算しているが、これと同様な手法で、姿勢角計算部４９３においても初期姿勢角を計算すればよい。この初期方向余弦マトリクスＣ_０は姿勢角計算部４９３内の記憶部に保持される。
【００２６】
このようにして求めた初期方向余弦マトリクスＣ_０を用いて、図１中の姿勢基準計算部１８で求めた姿勢基準である方向余弦ベクトルＣに対し、
Ｃ′＝Ｃ・Ｃ_０ ^Ｔ，Ｃ_０ ^ＴはＣ_０の転置行列
出力姿勢角設定部４９で計算し、リセット時の姿勢角分だけ回転させた方向余弦ベクトルＣ′を求め、このＣ′を用いてオイラー角を計算する。このようにして、図１１Ａに示したように例えばピッチ方向にΔθだけ傾けて姿勢角検出装置３９が取付けられても、図１１Ｂに示すように、初期姿勢角Ｃ_０により姿勢基準がΔθだけ回転補正され、期待しない軸に角速度が入力されるおそれがなく、それだけ正しい姿勢角を出力することができる。
【００２７】
このように初期姿勢角は加速度計出力により求める。従って加速度計にバイアス誤差がある場合は、正しい水平面を検出することができず初期姿勢角に誤差が発生する。リセットごとに姿勢角検出装置を水平に設置することは頭部３８などに取付けた後では難かしい。また出荷後に誤差が発生した場合も修正できるよう、利用者により設定することができるバイアス誤差の補正モードを用意するとよい。
【００２８】
つまり姿勢角検出装置３９をなるべく水平面に近い状態に置き、図６にも示すように、この補正モードにおいてはスイッチ４９４をオンにして加速度計出力を引算器４９５へ供給して重力加速度（０，０，１Ｇ）との差を求め、この差を加速度バイアス推定部４９６で必要に応じてＮ回平均化して加速度バイアスを推定し、この推定値を、不揮発性メモリ４９７に記憶する。その後、スイッチ４９８をオンにして、メモリ４９７よりの加速度バイアス補正値を引算器４９９に入力し、加速度計出力から加速度バイアス補正値を差し引き、その結果が重力加速度（０，０，１Ｇ）になるようにされる。このようにして、次回の電源ＯＮ時からは正しい加速度が引算器４９７の出力から得られるようにされる。
上述において各部はコンピュータによりプログラムを実行させて機能させることもできる。
【００２９】
【発明の効果】
以上述べたようにこの発明によれば大きな角速度バイアス誤差があってもジャイロ出力の有効なＡＤ変換レンジが狭まることがなく、ドリフトが生じない方位センサを用いることなく角速度バイアス誤差を推定することができる。

【図面の簡単な説明】
【図１】この発明の実施例の機能構成を示すブロック図。
【図２】Ａは図１中の第１及び第２補正器４１及び４２の具体例を示す図、Ｂ及びＣはその説明のための図である。
【図３】図１中の第１及び第２補正器４１及び４２の他の具体例を示す図。
【図４】図１中の角速度バイアス推定部４３の具体例を示す図。
【図５】図４中の角速度誤差判定処理部４３２における処理手順の一部の例を示す流れ図。
【図６】図１中の初期姿勢計算部４８の具体例を示す図。
【図７】従来の姿勢角検出装置を示すブロック図。
【図８】図７中の水平加速度ダンピング部２１の具体例を示す図。
【図９】その他の例を示す図。
【図１０】Ａは従来のジャイロバイアス補正の構成を示す図、Ｂはその説明のための図である。
【図１１】Ａは取付け状態が傾いた場合の必要な軸に角速度が生じる様子を示す図、Ｂはその補正を行った状態を示す図である。

Claims

ジャイロを備え、そのジャイロの出力により姿勢角を検出する姿勢角検出装置において、
ジャイロバイアス補正部を備え、
このジャイロバイアス補正部は、
リセットコマンドにより動作し、上記ジャイロの出力からジャイロバイアス誤差を推定する誤差推定部と、上記ジャイロの出力と上記ジャイロバイアス誤差との差をアナログ値として求める第１減算器とを有する第１補正器と、
上記第１補正器の誤差推定完了後に動作し、上記第１減算器の出力がデジタル値として入力され、そのデジタル値から補正残差を推定する補正残差推定部と、上記デジタル出力と上記補正残差との差をデジタル値として求め、上記姿勢角を求めるためのジャイロ出力とする第２減算器とを有する第２補正器とよりなることを特徴とする姿勢角検出装置。