JP3886005B2

JP3886005B2 - 姿勢角度検出装置

Info

Publication number: JP3886005B2
Application number: JP2002280383A
Authority: JP
Inventors: 和健武藤
Original assignee: NEC Tokin Corp
Current assignee: Tokin Corp
Priority date: 2002-09-26
Filing date: 2002-09-26
Publication date: 2007-02-28
Anticipated expiration: 2022-09-26
Also published as: JP2004117139A

Description

【０００１】
【発明の属する技術分野】
本発明は、ジャイロ、加速度センサおよび磁気センサを用いた小型の姿勢角度検出装置に関し、特に、移動体の姿勢検出や姿勢制御・バーチャルリアリティー技術等のうち、ヘッドマウントディスプレイにおける頭の姿勢角度を検出するトラッカー、３Ｄゲームパッド等、３次元空間中で被測定対象物の姿勢角度（３軸の回転角度）を検出するのに適した姿勢角度検出装置に関する。
【０００２】
【従来の技術】
従来、バーチャルリアリティー技術に使用される、頭の動きを検出する姿勢角検出方法として、外部に設置された交流磁場発生源により微弱交流磁場を発生し、ヘッドマウントディスプレイ（以下、ＨＭＤという）に配置したセンサ部で検知し、制御演算部で頭の動きを検出することによる交流磁場を利用する方法がある。
【０００３】
また、外部に設置した超音波発生源からの超音波信号をＨＭＤに配置したセンサ部で検知し、制御演算部で頭の動きを検出することによる超音波を利用する方法がある。
【０００４】
また、次の特許文献１には、ジャイロセンサ、加速度センサおよび地磁気センサを組み合わせてなる姿勢角度検出装置が示されている。
【０００５】
【特許文献１】
特開平１１−２１１４７４号公報
【０００６】
【発明が解決しようとする課題】
しかしながら、前者の交流磁場を利用した方法では、信号発生源が微弱交流磁場のため、ノイズをキャンセルする為にフィルタを多用することにより応答性が低下し、頭の動きに比べて、ＨＭＤの映像の動きが遅くなり、気分を悪くしたり、酔いを発生したりする場合がある。
【０００７】
また、後者の超音波を利用した方法では、他の様々な超音波信号の影響を受けての誤動作や、信号発生源とセンサとの間の遮蔽物により信号を検出できなくなる可能性があり、センサの前に腕や髪の毛があるだけで受信不能になるので、このための誤動作を防止するための対策を必要とする等の問題があった。
【０００８】
また、ジャイロセンサ、加速度センサおよび地磁気センサを組み合わせてなる姿勢角度検出装置についても、姿勢角度の検出対象となる移動体がほぼ静止している状態などにおいて、微小な出力の揺動が発生して、安定した姿勢角度が得られないということがあった。
【０００９】
そこで、本発明は、上記問題点を解消し、特に静止時の安定性が良く、応答性も良く、使用する場所や環境の制限を緩和し、幅広く使用できる、移動体の姿勢制御・検出に好適な姿勢角度検出装置を提供することを課題とする。
【００１０】
【課題を解決するための手段】
本発明の姿勢角度検出装置は、水平面内で直交するＸ軸、Ｙ軸、およびそのＸ軸、Ｙ軸に直交するＺ軸が形成する基準座標系での、少なくとも、Ｙ軸回りの傾斜角度とＺ軸回りの方位角度を検出する姿勢角度検出装置において、少なくとも、Ｙ軸回りおよびＺ軸回りの角速度を検出するためのジャイロと、Ｘ軸方向およびＹ軸方向の加速度を検出するための加速度センサと、Ｘ-Ｙ平面内の地磁気を検出するための磁気センサと、前記ジャイロの出力に基づいて、単位時間に運動した角度を演算する運動角検出装置と、前記加速度センサおよび地磁気センサの出力に基づいて、Ｙ軸およびＺ軸回りの回転角を演算する静止角演算装置と、この静止角演算装置による演算結果の真偽を判別する判別装置と、この判別装置の判別結果に応じて、前記運動角演算装置と静止角演算装置との演算結果から姿勢角度を演算する姿勢角演算装置と、この姿勢角演算装置から出力された姿勢角度に対して、不感帯を設け、微小振動を抑制して出力する不感帯処理装置とを備えることを特徴とする。
【００１１】
また、不感帯処理装置は、不感帯上限値、不感帯下限値および出力を記憶するメモリを有し、このメモリに記憶されている値、前記姿勢角演算装置から得られた姿勢角演算装置出力、および定められた不感帯幅を用いて、次の手順により、（１）姿勢角演算装置出力が、不感帯上限値より小さく、不感帯下限値より大きい場合は既に記憶されている出力値を今回の出力値として出力する、
（２）前記の姿勢角演算装置出力が不感帯上限値より大きいか等しい場合は、
（不感帯上限値）＝（姿勢角演算装置出力）、
（不感帯下限値）＝（不感帯上限値）−（不感帯幅）、
（出力）＝（不感帯上限値）−（不感帯幅）／２、
と演算して出力する、
（３）前記の姿勢角演算装置出力が不感帯下限値より小さいか等しい場合は、
（不感帯下限値）＝（姿勢角演算装置出力）、
（不感帯上限値）＝（不感帯下限値）＋（不感帯幅）、
（出力）＝（不感帯下限値）＋（不感帯幅）／２、
と演算して出力する共に、得られた値を前記メモリに更新して記憶し、この手順を繰り返して実行する手段を備えることができる。
【００１２】
【発明の実施の形態】
図４には、本発明の実施の一形態に係る姿勢角度検出装置において、基準座標系（Ｘ-Ｙ-Ｚ系）とセンサ座標系（Ｘs-Ｙs-Ｚs系）が一致している場合の、ジャイロ、加速度センサおよび磁気センサの配置を示す。
【００１３】
図４に示すように、水平面で直交する軸をＸ軸、Ｙ軸とし、そのＸ軸、Ｙ軸の両方に直交する軸をＺ軸とするとき、これら互いに直交するＸ軸、Ｙ軸、Ｚ軸が形成する基準座標系において、Ｘ軸回りの傾斜角度をロール角γ、Ｙ軸回りの傾斜角度をピッチ角β、Ｚ軸回りの方位角度をヨー角αと記す。そして、これらの変数を成分とする（α，β，γ）を姿勢角度と呼んでいる。このときの基準座標の＋Ｘ軸は北方向とする。
【００１４】
図４で示すセンサ座標系において、互いに直交する３軸（Ｘs軸、Ｙs軸およびＺs軸）の回りのそれぞれの角速度を検出するための第１のジャイロ１１、第２のジャイロ１２および第３のジャイロ１３は、それぞれ、Ｘs軸、Ｙs軸およびＺs軸に平行に、従って互いに直交して、配置されている。
【００１５】
また、第１の加速度センサ１４と第２の加速度センサ１５は、それぞれ、互いに直交する２軸、Ｘs軸およびＹs軸に平行に配置されている。同様に、第１の磁気センサ１６と第２の磁気センサ１７は、それぞれ、互いに直交する２軸、Ｘs軸およびＹs軸に平行に配置されている。
【００１６】
図１は、本発明の実施の一形態における姿勢角度検出装置の全体構成を示すブロック図である。ジャイロ２２ａ〜２２ｃ、加速度センサ２３ａと２３ｂ、および磁気センサ２４ａと２４ｂからはそれぞれ角速度（ωx，ωy，ωz）、加速度（Ａx，Ａy）、および地磁気（Ｍx，Ｍy）が出力される。
【００１７】
運動角演算装置２６ａはジャイロ２２ａ〜２２ｃから出力される角速度（ωx，ωy，ωz）から検出対象が単位時間に運動した角度である運動角（Δα，Δβ，Δγ）を演算し出力する。
【００１８】
静止角演算装置２６ｂは、加速度センサ２３ａと２３ｂとから出力される加速度（Ａx，Ａy）と、磁気センサ２４ａと２４ｂとから出力される地磁気（Ｍx，Ｍy）とから仮の姿勢角度である静止角（φ，Ｐ，Ｒ）を演算し出力する。
【００１９】
判別装置２７は得られた運動角と静止角を比較して静止角が正しいかどうかを判別し、姿勢角演算装置２６ｃに結果を出力する。
【００２０】
姿勢角演算装置２６ｃは運動角（Δα，Δβ，Δγ）、静止角（φ，Ｐ，Ｒ）と判別装置の出力とから仮の姿勢角度（α′，β′，γ′）を演算し、不感帯処理装置に出力する。
【００２１】
不感帯処理装置２８は姿勢角演算装置から得られた仮の姿勢角度（α′，β′，γ′）に演算を行い、真の姿勢角度（α，β，γ）を出力端２９に出力する。
【００２２】
次に、各演算装置を詳細に説明する。
【００２３】
最初に、運動角演算装置を説明する。姿勢角演算装置に加わる角速度に応じた各ジャイロの出力（ωx，ωy，ωz）は、座標変換を行って、演算する単位時間で１単位時間だけ過去の出力β（ｎ−１）およびγ（ｎ−１）とから、次式の如く、基準座標系における現在の運動角（Δα，Δβ，Δγ）に変換される（ただし、ｎは自然数）。
【００２４】
Δα(ｎ）＝ωy・ｓｉｎ{γ(ｎ−１）}／ｃｏｓ{β(ｎ−１）}＋ωz・ｃｏｓ{γ(ｎ−１）}／ｃｏｓ{β(ｎ−１）}，
Δβ(ｎ）＝ωy・ｃｏｓ{γ(ｎ−１）}−ωz・ｓｉｎ{γ(ｎ−１）}，
Δγ(ｎ）＝ωx＋ωy・ｔａｎ{β(ｎ−１）}・ｓｉｎ{γ(ｎ−１）}＋ωz・ｔａｎ{β(ｎ−１）}・ｃｏｓ{γ(ｎ−１）}.
【００２５】
次に、図２の静止角演算装置の説明を行う。図４に示した加速度センサ１４および１５は、それぞれセンサ座標系での重力加速度のＸs成分Ａx(ｎ）およびＹs成分Ａy(ｎ）を検出するように配置されている。
【００２６】
また、図４に示した第１の磁気センサ１６および第２の磁気センサ１７は、それぞれＸs軸およびＹs軸上の地磁気成分Ｍx(ｎ）およびＭy(ｎ）を検出するように配置されている。
【００２７】
加速度センサの出力Ａx(ｎ）およびＡy(ｎ）は、ピッチ角演算手段およびロール角演算手段にて次式により、基準座標系の水平面(Ｘ-Ｙ平面）との傾斜角である仮のピッチ角Ｐ(ｎ）および仮のロール角Ｒ(ｎ）を算出する。
【００２８】
Ｐ(ｎ）＝ｓｉｎ^−１{Ａx(ｎ）},
Ｒ(ｎ）＝ｓｉｎ^−１[−Ａy(ｎ）／ｃｏｓ{Ｐ(ｎ)}].
【００２９】
基準座標系での仮のヨー角φ(ｎ）は、基準座標系での地磁気成分ＨＸ(ｎ）、ＨＹ(ｎ）を用いて、
φ(ｎ）＝−ｔａｎ^−１{ＨＹ(ｎ）／ＨＸ(ｎ）}−φ(０），
で表される。
【００３０】
ただし、φ(０）は原点時の方位角である。姿勢角度検出装置が水平面上で回転する場合は、磁気センサの出力Ｍx(ｎ）およびＭy(ｎ）が、それぞれ、ＨＸ(ｎ）およびＨＹ(ｎ）に等しく、
φ(ｎ）＝−ｔａｎ^−１{Ｍy(ｎ）／Ｍx(ｎ）}−φ(０），
が成り立つ。
【００３１】
しかし、姿勢角度検出装置がピッチ角Ｐやロール角Ｒを持ち、水平でないときには、
ＨＸ(ｎ）＝Ｍx・ｃｏｓ{Ｐ(ｎ）}＋[ Ｍy・ｓｉｎ{Ｒ(ｎ）}＋Ｍz・ｃｏｓ{Ｒ(ｎ）} ]・ｓｉｎ{Ｐ(ｎ）}，
ＨＹ(ｎ）＝Ｍy・ｃｏｓＲ(ｎ）−Ｍz・ｓｉｎＲ(ｎ），
の関係式から、仮のピッチ角Ｐ(ｎ）および仮のロール角Ｒ(ｎ）を用いて座標変換を行い、基準座標系での地磁気成分を求めて、
φ(ｎ）＝−ｔａｎ^−１{ＨＹ(ｎ）／ＨＸ(ｎ）}−φ(０），
に代入し、求める。その際、仮想磁気Ｚ方向成分Ｍz(ｎ）が必要となる。
【００３２】
そのために、以下の方法で仮想的にＭzを算出する。まず、地磁気の俯角θの初期値をメモリに保存しておく、この初期値は姿勢角度検出装置の使用場所において測定することが望ましい。
【００３３】
次に、地磁気の絶対値Ｈtを以下の手順で求める。姿勢角度検出装置（被測定物）を原点位置（水平）にし、何らかの信号を送り、例えばキーやボタンを押すなどして、磁気センサＭx、Ｍyの値をＭx(０）、Ｍy(０）としてメモリに格納する。Ｍx(０）、Ｍy(０）、θを用いて、Ｈtは次式から求まる。
【００３４】
Ｈt ＝{Ｍx(０)²＋Ｍy(０)²}^1/2／ｃｏｓ{θ}.
【００３５】
次に、姿勢角度検出装置（被測定物）の北方向成分傾斜角から仮想磁気Ｍzの符号を判別する。姿勢角演算装置にて算出した、演算する単位時間で、１単位時間過去のα(ｎ−１）、β(ｎ−１）およびγ(ｎ−１）をバックアップメモリに記憶しておき、このヨー角α(ｎ−１）、ピッチ角β(ｎ−１）およびロール角γ(ｎ−１）から、北方向成分傾斜角演算手段で、姿勢角度検出装置（被測定物）の北方向成分傾斜角θ₂を以下の手順で求める。
【００３６】
基準座標Ｘ-Ｚ平面とセンサ座標Ｘs-Ｙs平面とが交差する直線の方向ベクトルを（ａａ，０，ｃｃ）とすると、
ａａ＝ｃｏｓ{γ(ｎ−１）}・ｓｉｎ{β(ｎ−１）}・ｃｏｓ{φ(ｎ−１）＋φ(０）}＋ｓｉｎ{γ(ｎ−１）}・ｓｉｎ{φ(ｎ−１）＋φ(０）］,
ｃｃ＝ｃｏｓ{γ(ｎ−１）}・ｃｏｓ{β(ｎ−１）},
となり、北方向成分傾斜角θ₂は
θ₂＝ｓｉｎ^−１{ａａ／（ａａ²＋ｃｃ²）^1/2},
と求められる。
【００３７】
このとき、−９０度≦θ₂≦θであれば、仮想磁気Ｍz符号はマイナス、θ＜θ₂≦９０度であれば、仮想磁気Ｍz符号はプラスと判別する。
【００３８】
次に、仮想磁気Ｍzの絶対値を求める。地磁気の絶対値Ｈtと磁気センサの出力Ｍx(ｎ）、Ｍy(ｎ）の値から仮想磁気Ｍzの絶対値を次式により算出する。
【００３９】
｜Ｍz｜＝{Ｈt²−（Ｍx²＋Ｍy²）}^1/2 .
【００４０】
以上のように仮想磁気Ｍzが求まったことにより、前述したＨＸ(ｎ）、ＨＹ(ｎ）が求まり、従ってφ(ｎ）を求めることができる。
【００４１】
次に、本発明の姿勢角度検出装置における判別装置を説明する。
【００４２】
加速度センサの出力Ａx、Ａyは重力加速度のＸs、Ｙs軸方向成分と運動加速度のＸs、Ｙs軸方向成分との合成ベクトルとなるために、運動加速度がある場合には静止角演算装置によって算出された仮のピッチ角Ｐおよび仮のロール角Ｒ(ｎ）は正しいピッチ角にはならない。
【００４３】
また、仮のヨー角φについても磁場環境によって正しいヨー角にならない場合がある。判別装置は静止角演算装置の出力であるφ(ｎ）、Ｐ(ｎ）、Ｒ(ｎ）が正しいかどうかを判別するものである。
【００４４】
演算する単位時間で、１単位時間に変化したφ、Ｐ、Ｒを、Δφ、ΔＰ、ΔＲとし、これらを運動角演算装置の出力Δα、Δβ、Δγと比較する。
【００４５】
微小定数εを用いて、
｜Δφ−Δα｜＜ε，
｜ΔＰ−Δβ｜＜ε，
｜ΔＲ−Δγ｜＜ε，
が成立するならば、それぞれφ、Ｐ、Ｒは正しいと判断する。
【００４６】
次に、上記判別装置の判別結果に応じて、運動角演算装置の出力と静止角演算装置の出力を用いて姿勢角演算装置で行われる演算方法について説明する。
【００４７】
この判別装置においてφ、Ｐ、Ｒが正しいと判断された場合には、ｋを比例定数として、
α′(ｎ）＝α′(ｎ−１）＋Δα−ｋ・{α′(ｎ−１）＋Δα−φ(ｎ）}，
β′(ｎ）＝β′(ｎ−１）＋Δβ−ｋ・{β′(ｎ−１）＋Δβ−Ｐ(ｎ）}，
γ′(ｎ）＝γ′(ｎ−１）＋Δγ−ｋ・{γ′(ｎ−１）＋Δγ−Ｒ(ｎ）}，
により姿勢角度を演算する。
【００４８】
他方、判別装置においてφ、Ｐ、Ｒが誤っていると判断された場合には、
α′(ｎ）＝α′(ｎ−１）＋Δα，
β′(ｎ）＝β′(ｎ−１）＋Δβ，
γ′(ｎ）＝γ′(ｎ−１）＋Δγ，
により姿勢角度を演算する。
【００４９】
最後に、図３のフローチャートに示された本発明における不感帯処理装置について説明する。
【００５０】
不感帯処理装置のバックアップメモリには不感帯幅Ｎｍｐと姿勢角度（α，β，γ）のそれぞれに対応した不感帯上限値（Ｎｍlα，Ｎｍlβ，Ｎｍlγ），不感帯下限値（Ｎｍsα，Ｎｍsβ，Ｎｍsγ）および前回の姿勢角度の出力値（α(ｎ−１），β(ｎ−１），γ(ｎ−１））が記憶されている。
【００５１】
まず、ヨー角αの場合について説明する。この流れは図３（ｂ）に示すようである。
【００５２】
（１）姿勢角演算装置の出力ヨー角α′(ｎ）がＮｍlαより小さく、Ｎｍsαより大きい場合は、α（ｎ）として、前回の出力値α(ｎ−１）を出力する。
【００５３】
この流れは、図３（ａ）において、ステップ４１、ステップ４３およびステップ４５に示した。
【００５４】
また、（２）姿勢角演算装置の出力ヨー角α′(ｎ）がＮｍlαより大きいか等しい場合は、
Ｎｍlα＝α′(ｎ），
Ｎｍsα＝Ｎｍlα−Ｎｍｐ，
α(ｎ）＝Ｎｍlα−Ｎｍｐ／２，
と演算して出力する。
【００５５】
この流れは、ステップ４１およびステップ４２に示した。
【００５６】
また、（３）姿勢角演算装置の出力ヨー角α′(ｎ）がＮｍsαより小さいか等しい場合は、
Ｎｍsα＝α′(ｎ），
Ｎｍlα＝Ｎｍsα＋Ｎｍｐ，
α(ｎ）＝Ｎｍsα＋Ｎｍｐ／２，
と演算し、出力する。
【００５７】
この流れはステップ４３およびステップ４４に示した。
【００５８】
変更したＮｍlα、Ｎｍsαおよび出力α（ｎ）はバックアップメモリに保存する。このステップは４６に示した。
【００５９】
その後、開始に戻り、同じ手順の処理を繰り返す。
【００６０】
ピッチ角βおよびロール角γについても同様の処理を行う。すなわち、全体として、図３（ａ）に示すような不感帯処理を行う。
【００６１】
このような不感帯処理により、微小な出力変動を抑制して、安定した姿勢角度が得られるようになった。
【００６２】
【発明の効果】
以上に説明したように、本発明によれば、不感帯処理によって手ぶれやノイズを原因とする微小な動きをキャンセルして、不感帯幅として設定されたある一定幅以上の動きにのみ反応することにより、静止時の安定性に優れた姿勢角度検出装置が得られた。
【００６３】
また、応答性が良く、使用する場所や環境の制限を緩和し、幅広く使用できる移動体の姿勢制御・検出に用いる姿勢角度検出装置が得られた。
【図面の簡単な説明】
【図１】本発明の実施の一形態における姿勢角度検出装置の全体構成を示すブロック図。
【図２】本発明の実施の一形態における静止角演算装置の構成に関するブロック図。
【図３】本発明の実施の一形態における不感帯処理装置の処理を示すフローチャート。
【図４】本発明の実施の一形態における姿勢角度検出装置のセンサ構成を示す図。
【符号の説明】
１１第１のジャイロ
１２第２のジャイロ
１３第３のジャイロ
１４第１の加速度センサ
１５第２の加速度センサ
１６第１の磁気センサ
１７第２の磁気センサ
２２ａ，２２ｂ，２２ｃジャイロ
２３ａ，２３ｂ加速度センサ
２４ａ，２４ｂ磁気センサ
２６ａ運動角演算装置
２６ｂ静止角演算装置
２６ｃ姿勢角演算装置
２７判別装置
２８不感帯処理装置
２９出力端
４１〜４６ステップ

Claims

水平面内で直交するＸ軸、Ｙ軸、およびそのＸ軸、Ｙ軸に直交するＺ軸が形成する基準座標系での、少なくとも、Ｙ軸回りの傾斜角度とＺ軸回りの方位角度を検出する姿勢角度検出装置において、少なくとも、Ｙ軸回りおよびＺ軸回りの角速度を検出するためのジャイロと、Ｘ軸方向およびＹ軸方向の加速度を検出するための加速度センサと、Ｘ-Ｙ平面内の地磁気を検出するための磁気センサと、前記ジャイロの出力に基づいて、単位時間に運動した角度を演算する運動角検出装置と、前記加速度センサおよび地磁気センサの出力に基づいて、Ｙ軸およびＺ軸回りの回転角を演算する静止角演算装置と、この静止角演算装置による演算結果の真偽を判別する判別装置と、この判別装置の判別結果に応じて、前記運動角演算装置と静止角演算装置との演算結果から姿勢角度を演算する姿勢角演算装置と、この姿勢角演算装置から出力された姿勢角度に対して、不感帯を設け、微小振動を抑制して出力する不感帯処理装置とを備える姿勢角度検出装置であって、前記不感帯処理装置は、不感帯上限値、不感帯下限値および出力を記憶するメモリを有し、このメモリに記憶されている値、前記姿勢角演算装置から得られた姿勢角演算装置出力、および定められた不感帯幅を用いて、次の手順により、
（１）姿勢角演算装置出力が、不感帯上限値より小さく、不感帯下限値より大きい場合は既に記憶されている出力値を今回の出力値として出力する、
（２）前記の姿勢角演算装置出力が不感帯上限値より大きいか等しい場合は、
（不感帯上限値）＝（姿勢角演算装置出力）、
（不感帯下限値）＝（不感帯上限値）−（不感帯幅）、
（出力）＝（不感帯上限値）−（不感帯幅）／２、
と演算して出力する、
（３）前記の姿勢角演算装置出力が不感帯下限値より小さいか等しい場合は、
（不感帯下限値）＝（姿勢角演算装置出力）、
（不感帯上限値）＝（不感帯下限値）＋（不感帯幅）、
（出力）＝（不感帯下限値）＋（不感帯幅）／２、
と演算して出力する共に、得られた値を前記メモリに更新して記憶し、この手順を繰り返して実行する手段を備えることを特徴とする姿勢角度検出装置。