JPH11248456A - ３軸姿勢検出装置 - Google Patents

Abstract

(57)【要約】 【課題】頭部装着型のディスプレイ装置、或いは、航空
機、その他の種々の目的物体の空間内での姿勢を、ピッ
チ軸、ロール軸及びヨー軸等の該目的物体に関して仮定
された相互に交差する３軸の各軸の回りの回動角として
検出する３軸姿勢検出装置の構成の簡素化を計り、且
つ、検出精度を向上させる。 【解決手段】第１乃至第３のジャイロセンサ（１１〜１
３）による回動角の検出値および第１乃至第３の磁気セ
ンサ（２１〜１３）による方位角の検出値に基づいて、
第１乃至第３のジャイロセンサ（１１〜１３）の出力た
る上記二次的検出値としての回動角の検出値に含まれる
一次的検出値の誤差分に関する経時的積分値であるドリ
フト成分の値をドリフト推定演算回路（５０）で推定
し、ドリフト補正演算回路（６０）で第１乃至第３の磁
気センサ（２１〜１３）による検出値に対しドリフト成
分の推定値を適用して誤差が補正された３軸姿勢検出出
力を得る

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、例えば、頭部装着
型のディスプレイ装置、或いは、航空機、その他の種々
の目的物体の空間内での姿勢を、ピッチ軸、ロール軸及
びヨー軸等の該目的物体に関して仮定された相互に交差
する３軸の各軸の回りの回動角として検出する３軸姿勢
検出装置に関する。

【０００２】尚、頭部装着型のディスプレイ装置（Head
Mounted Display：ＨＭＤと略称される）に姿勢検出器
を備えることについては、例えば、米国特許第5,373,85
7 号明細書、特開平3-289615号公報等に開示がある。

【０００３】

【従来の技術】従来のこの種の３軸姿勢検出装置として
は、ジャイロを上記３軸の各軸に対応させて３個直交す
るように配置し、ピッチ軸、ロール軸及びヨー軸の回り
の回動角を検出するものがあった。また、同様の方式の
３軸姿勢検出装置で、頭部装着型のディスプレイ装置の
姿勢検出にも適合する小型のシステムとしては、日本航
空電子株式会社製のＪＭＩＳ３０Ｓマックスキューブ
（商標）という検出器の応用技術として既に公知のもの
がある。

【０００４】また、この種の３軸姿勢検出装置として、
傾斜センサと磁気センサとを併用したシステムもある。
例えば、米国プレシジョンナビゲーション社のシステム
のようにいずれも液体式（密閉容器内の液体の液面の動
きで傾きを検出する方式）の２軸の傾斜センサと磁気セ
ンサ３個を併用し、傾斜センサでピッチ軸及びロール軸
回りの回動角を検出し、磁気センサでヨー軸の回りの回
動角を検出するものである。尚、株式会社ワコーから
は、半導体型３軸加速度センサＰＲＧシリーズ（商標）
という、非液体式の検出器が提供され、このセンサを傾
斜センサとして用いる方式も考えられる。

【０００５】更に、株式会社データ・テックからは、振
動ジャイロ３個と加速度センサを３個（３軸）用いて頭
部装着型のディスプレイ装置の姿勢検出に適合する小型
のシステムが提供されている。

【０００６】上述の、ジャイロをピッチ軸、ロール軸及
びヨー軸の回りの回動角を検出するように３個直交する
ように配置する方式のものでは、ジャイロセンサ自体が
回動角の検出の基礎となる角速度を一次的に検出しこの
一次的検出値の経時的積分値である二次的検出値として
これら回動角を検出するものであるため、次のような問
題がある。即ち、一次的検出対象である角速度の真値が
０であるときに、センサ出力としてこれからずれた値を
示すような特性を呈するとき、換言すれば、検出データ
にオフセットが含まれるときには、これの経時的積分値
である二次的検出値としての回動角のデータは、時間の
経過と共に益々その誤差の幅が拡大されてしまうことに
なる。即ち、ジャイロセンサによる場合は、回動角のデ
ータにドリフトが生じてしまい、これによる誤差は時間
の経過と共に拡大してしまうことになる。

【０００７】一方、液体式の傾斜センサと磁気センサと
を併用したシステムでは、既述のように、傾斜センサに
よってピッチ軸及びロール軸回りの回動角を検出し、磁
気センサによってヨー軸の回りの回動角を検出する。こ
の場合、傾斜センサは重力加速度を基準としているた
め、平行移動による誤差が生じる。また、液体式の傾斜
センサは密閉容器内の液体の液面の傾きに依拠して検出
値を得るという構造上、加速度が作用している間は検出
誤差を生じる（慣性力による誤差）。このため、速度が
定常となるとき、即ち、この加速度が０となるときを待
って検出値を得る必要があることから、検出に遅れが生
じてしまう。尚、非液体式の傾斜センサを適用する方式
を採った場合も、平行移動による誤差は不可避である。

【０００８】また、振動ジャイロ３個と加速度センサを
３個（３軸）用いる方式では、加速度センサを３個（３
軸）用いて傾斜センサを構成し、ジャイロのドリフトを
補正するようにしている。しかしながら、傾斜センサに
よる場合、ピッチ軸及びロール軸回りの回動角しか検出
できないため、ヨー軸の回りの回動角に関するドリフト
を補正することができない。これを補正するためには、
更に磁気センサを３個付加してヨー軸に係るドリフトを
補正することも考えられるが、センサの数が極めて多く
なり、システムが複雑化し、大型化が避けられず、コス
トも高くなってしまう。

【０００９】

【発明が解決しようとする課題】本願発明は叙上のよう
な事情に鑑みてなされたものであり、上述のドリフト等
に起因する検出誤差が少なく、且つ構成が簡単なこの種
の３軸姿勢検出装置を提供しようとするものである。

【００１０】

【課題を解決するための手段および作用】上記課題を解
決するための、一つの本願発明は：目的物体の空間内で
の姿勢を検出するために、該目的物体に関して仮定され
た相互に交差する３軸の各軸の回りの回動角について、
該回動角の検出の基礎となる角速度を一次的に検出しこ
の一次的検出値の経時的積分値である二次的検出値とし
てこれら回動角を検出すべく該３軸のうちの各該当する
軸に対応して設けられた第１のジャイロセンサ、第２の
ジャイロセンサ、及び第３のジャイロセンサと、上記３
軸の各軸の回りの回動角について、該回動角の検出の基
礎となる方位角を検出すべく該３軸のうちの各該当する
軸に対応して設けられた第１の磁気センサ、第２の磁気
センサ、及び第３の磁気センサと、上記第１乃至第３の
磁気センサによる検出値に対し上記第１乃至第３のジャ
イロセンサによる検出値に依拠して得たデータを所定の
演算により適用して誤差が補正された３軸姿勢検出出力
を得る演算手段と、を備えてなることを特徴とする３軸
姿勢検出装置というものである……［１］

【００１１】上記［１］の発明によれば、誤差の少ない
３軸姿勢検出装置が実現され、且つ、構成の簡素化とコ
ストの低減も図られる。

【００１２】また、他の一つの本願発明は：目的物体の
空間内での姿勢を検出するために、該目的物体に関して
仮定された相互に交差する３軸の各軸の回りの回動角に
ついて、該回動角の検出の基礎となる角速度を一次的に
検出しこの一次的検出値の経時的積分値である二次的検
出値としてこれら回動角を検出すべく該３軸のうちの各
該当する軸に対応して設けられた第１のジャイロセン
サ、第２のジャイロセンサ、及び第３のジャイロセンサ
と、上記３軸の各軸の回りの回動角について、該回動角
の検出の基礎となる方位角を検出すべく該３軸のうちの
各該当する軸に対応して設けられた第１の磁気センサ、
第２の磁気センサ、及び第３の磁気センサと、上記第１
乃至第３のジャイロセンサによる回動角の検出値および
上記第１乃至第３の磁気センサによる方位角の検出値に
基づいて、上記第１乃至第３のジャイロセンサの出力た
る上記二次的検出値としての回動角の検出値に含まれる
上記一次的検出値の誤差分に関する経時的積分値である
ドリフト成分の値を推定するドリフト推定演算手段と、
上記第１乃至第３の磁気センサによる検出値に対し上記
ドリフト推定演算手段により算出されたドリフト成分の
推定値を適用して誤差が補正された３軸姿勢検出出力を
得るドリフト補正演算手段と、を備えてなることを特徴
とする３軸姿勢検出装置というものである……［２］

【００１３】上記［２］の発明によれば、誤差の少ない
３軸姿勢検出装置が実現され、且つ、構成の簡素化とコ
スト低減も図られる。特にこの発明では、ジャイロセン
サによるドリフトの影響が確実に補正された出力が得ら
れる。

【００１４】また、更に、他の一つの本願発明は：目的
物体の空間内での姿勢を検出するために、該目的物体に
関して仮定された相互に交差する３軸の各軸の回りの回
動角について、該回動角の検出の基礎となる角速度を一
次的に検出しこの一次的検出値の経時的積分値である二
次的検出値としてこれら回動角を検出すべく該３軸のう
ちの各該当する軸に対応して設けられた第１のジャイロ
センサ、第２のジャイロセンサ、及び第３のジャイロセ
ンサを含んで構成されたジャイロセンサ系検出手段と、
上記３軸の各軸の回りの回動角について、該回動角の検
出の基礎となる方位角を検出すべく該３軸のうちの各該
当する軸に対応して設けられた第１の磁気センサ、第２
の磁気センサ、及び第３の磁気センサを含んで構成され
た磁気センサ系検出手段と、上記ジャイロセンサ系検出
手段による回動角の検出値たるジャイロセンサ系検出デ
ータおよび上記磁気センサ系検出手段による方位角の検
出値たる磁気センサ系検出データを、これらジャイロセ
ンサ系検出データの表わす事象と磁気センサ系検出デー
タの表わす事象との両事象の時間的同時性が実効的に計
られるように適用して、上記ジャイロセンサ系検出デー
タたる上記二次的検出値としての回動角の検出値に含ま
れる上記一次的検出値の誤差に関する経時的積分値であ
るドリフト成分の値を推定するドリフト推定演算手段
と、上記磁気センサ系検出データに対し上記ドリフト推
定演算手段により算出されたドリフト成分の推定値を適
用して誤差が補正された３軸姿勢検出出力を得るドリフ
ト補正演算手段と、上記ジャイロセンサ系検出手段に対
する磁気センサ系検出手段の検出の応答遅れに相応する
時間以上の時間区間に亘って実効的に遅延が与えられて
いない状態での上記ジャイロセンサ系検出データを蓄積
可能なデータ格納用メモリと、上記データ格納用メモリ
に蓄積されたジャイロセンサ系検出データに依拠して上
記磁気センサ系検出手段の当該応答遅れに相応する時間
内での上記３軸姿勢検出出力の変化量の予測値を算出す
る変化量予測演算手段と、上記ドリフト補正演算手段の
出力たる３軸姿勢検出出力に上記変化量予測演算手段で
算出された３軸姿勢検出出力の変化量の予測値を加味し
て実効的に時間軸を遡及したと等価な３軸姿勢検出出力
を得る時間軸シフト演算手段と、備えてなることを特徴
とする３軸姿勢検出装置というものである………［３］

【００１５】上記［３］の発明によれば、誤差の少ない
３軸姿勢検出装置が実現され、且つ、構成の簡素化とコ
スト低減も図られる。特にこの発明では、磁気センサ系
での応答遅れによる遅延時間の影響を、時間軸の遡及的
シフト効果を奏する補正をかけて最小限のものとするこ
とによって、磁気ノイズの比較的多い環境下において磁
気センサ系にフィルタ等を介挿させた場合等においても
応答性に優れた姿勢検出装置を実現することができる。

【００１６】また、他の一つの本願発明は：上記第１の
ジャイロセンサ、第２のジャイロセンサ、及び第３のジ
ャイロセンサの上記一次的検出値たる上記３軸の各軸の
回りの角速度のデータに基づいて上記二次的検出値たる
回動角を回転量とこれに係る回転軸を表わすデータとし
て出力するように構成されたジャイロ回転量・回転角演
算手段と、上記第１の磁気センサ、第２の磁気センサ、
及び第３の磁気センサの出力に基づいて回転軸を表わす
データを出力するように構成された磁気センサ回転軸変
換手段と、を備えたことを特徴とする上記［１］，
［２］または［３］に記載の３軸姿勢検出装置というも
のである………………………………………………………
［４］

【００１７】上記［４］の発明によれば、ジャイロセン
サの出力を回転量とこれに係る回転軸を表わすデータに
変換し、磁気センサの出力を回転軸を表わすデータに変
換してから、検出誤差（ジャイロセンサに関するドリフ
ト）の推定を行うことができるため、このための演算が
簡素化される。

【００１８】

【発明の実施の形態】以下、図面を用いて本願発明の実
施の形態について詳述することにより本発明を明らかに
する。

【００１９】図１は本願発明の一つの実施の形態として
の３軸姿勢検出装置を示すブロック図である。図１にお
いて、例えば、航空機、工業用ロボット、または、ＨＭ
Ｄのような目的物体の空間内での姿勢を検出するため
に、該目的物体に関して仮定された相互に交差する３軸
（ｘ，ｙ，ｚ）の各軸の回りの回動角について、該回動
角の検出の基礎となる角速度を一次的に検出しこの一次
的検出値の経時的積分値である二次的検出値としてこれ
ら回動角を検出すべく該３軸（ｘ，ｙ，ｚ）のうちｘ軸
に対応して第１のジャイロセンサ１１が設けられ、ｙ軸
に対応して第２のジャイロセンサ１２が設けられ、ｚ軸
に対応して第３のジャイロセンサ１３が設けられてい
る。

【００２０】また、上記３軸（ｘ，ｙ，ｚ）の各軸の回
りの回動角について、該回動角の検出の基礎となる方位
角を検出すべく該３軸のうちｘ軸に対応して第１の磁気
センサ２１が設けられ、ｙ軸に対応して第２の磁気セン
サ２２が設けられ、ｚ軸に対応して第３の磁気センサ２
３が設けられている。

【００２１】図２は、上記第１乃至第３のジャイロセン
サ１１，１２，１３、及び、第１乃至第３の磁気センサ
２１，２２，２３の配置と、上記目的物体と一緒に動く
相互に直交する３軸（ｘ，ｙ，ｚ：以下センサ座標ｘｙ
ｚという）、並びに、基準となる動かない直交座標系で
ある外部座標ＸＹＺとの関係を示す模式図である。基準
となる外部座標ＸＹＺは演算を簡素化するために、Ｘ軸
方向が地磁気ベクトルの方向と一致するように設定され
ている。このセンサ座標ｘｙｚ及び外部座標ＸＹＺとの
関係において上記の第１乃至第３の磁気センサ２１，２
２，２３による検出データをより厳密に言えば次の通り
である。即ち、第１の磁気センサ２１はセンサ座標ｘｙ
ｚのｘ軸に地磁気ベクトルを投影した値を検出データと
して出力し、第２の磁気センサ２２はセンサ座標ｘｙｚ
のｙ軸に地磁気ベクトルを投影した値を検出データとし
て出力し、第３の磁気センサ２３はセンサ座標ｘｙｚの
ｚ軸に地磁気ベクトルを投影した値を検出データとして
出力する。

【００２２】上記第１乃至第３のジャイロセンサ１１，
１２，１３は上記各回動角の検出値を得るための基礎と
なる３軸（ｘ，ｙ，ｚ）の各軸の回りの角速度の検出デ
ータを一次的に生成する。図示の実施の形態では、これ
ら一次的検出データである各角速度の検出データは、第
１乃至第３のジャイロセンサ１１，１２，１３の内部の
回路でそれらデータの各経時的積分値を得るための演算
に付され、センサ座標ｘｙｚの各軸の回りの回動角を表
わす二次的検出値のデータＪx ，Ｊy ，Ｊz として出力
される。これらの出力データＪx ，Ｊy ，Ｊz は、夫々
に対応する増幅器１４，１５，１６、並びに、これらの
増幅器１４，１５，１６の各後段に設けられたＡ／Ｄ変
換器１７，１８，１９を介して、ディジタルデータに変
換され、ジャイロ回転量・回転軸演算回路３０に夫々供
給される。ジャイロ回転量・回転軸演算回路３０は、上
記供給されたデータに基づいて回転量とこの回転量に係
る回転軸を求める演算を実行する。尚、上記第１乃至第
３のジャイロセンサ１１，１２，１３、増幅器１４，１
５，１６及びＡ／Ｄ変換器１７，１８，１９はジャイロ
センサ系検出手段１０を構成する。

【００２３】一方、上記第１乃至第３の磁気センサ２
１，２２，２３による方位角の各検出値ＧX ，Ｇy ，Ｇ
z は、各対応する増幅器２４，２５，２６、並びに、こ
れらの増幅器２４，２５，２６の各後段に設けられたＡ
／Ｄ変換器２７，２８，２９を介して、ディジタルデー
タに変換され、磁気センサ回転量演算回路４０に夫々供
給される。尚、上記第１乃至第３の磁気センサ２１，２
２，２３、増幅器２４，２５，２６及びＡ／Ｄ変換器２
７，２８，２９は磁気センサ系検出手段２０を構成す
る。

【００２４】磁気センサ回転軸演算回路４０では、磁気
センサ系検出手段２０から供給されるデータに基づい
て、回転軸を求めるための演算を実行する。この磁気セ
ンサ回転軸演算回路４０の出力及び上記ジャイロ回転量
・回転軸演算回路３０の出力は、これらの出力に基づい
て（結局、上記出力データＪx ，Ｊy ，Ｊz 及びＧX ，
Ｇy ，Ｇz に基づいて）、上記第１乃至第３のジャイロ
センサの出力たる上記二次的検出値としての回動角の検
出値（センサ座標ｘｙｚの各軸の回りの回動角を表わす
二次的検出値のデータＪx ，Ｊy ，Ｊz ）に含まれる上
記一次的検出値（角速度検出値）の誤差分に関する経時
的積分値であるドリフト成分の値を推定するドリフト推
定演算回路５０に供給される。尚、ドリフトの推定につ
いては後に詳述する。

【００２５】また一方、上述したＡ／Ｄ変換器２７，２
８，２９の出力はドリフト補正演算回路６０にも供給さ
れる。このドリフト補正演算回路６０にはまた、上記ド
リフト推定演算回路５０の出力も供給される。ドリフト
補正演算回路６０は、第１乃至第３の磁気センサによる
検出値ＧX ，Ｇy ，Ｇz に対し上記ドリフト推定演算手
段５０により算出されたドリフト成分の推定値を適用し
て誤差が補正された３軸姿勢検出出力を得る。

【００２６】ドリフト補正演算回路６０によって誤差が
補正された回転軸・回転量の表現による姿勢検出出力
は、次段の角度データ変換回路７０に供給され、ここで
ピッチ軸、ロール軸及びヨー軸回りの回動角による表現
に変換され、更に次段の通信インタフェース回路８０を
介して適宜の映像信号処理装置等の外部のシステム（本
例のシステムの場合はホスト側コンピュータ）に伝送さ
れ、所定の制御や信号処理動作等を行なうに供される。

【００２７】以上、図１の回路において行われる各種の
演算について、次に数式を用いて説明する。既述のよう
に、ジャイロ回転量・回転軸演算回路３０ではジャイロ
センサ系検出手段１０から供給されるデータに基づいて
回転量とこの回転量に係る回転軸を求める演算を実行す
る。この演算のうち、上記の回転量Ｊs を求めるための
演算は次の（１）式に基づき実行される。

【数１】 また、上記回転量Ｊs に係る回転軸Ｊk を求めるための
演算は次の（２）式に基づき実行される。

【数２】

【００２８】次に、磁気センサ回転軸演算回路４０にお
いて、磁気センサ系検出手段２０から供給されるデータ
に基づいて、回転軸Ｇk を求めるための演算は次のよう
にして行われる。先ず、回転軸ベクトルＧk をＧk ＝
（Ｇkx，Ｇky，Ｇkz）として表わすと、Ｇkx、Ｇky、及
び、Ｇkzは、夫々次の（３）式、（４）式、及び（５）
式のようにして算出される。

【数３】

【数４】

【数５】

【００２９】磁気センサからは一意に姿勢を求めること
はできないため、上記のように回転軸ベクトルＧk をθ
の関数として表している。ジャイロにドリフトがなけれ
ば、θにＪs を代入すれば、Ｇk とＪk は等しくなる。
ある基準の姿勢から、現在の姿勢への変化を、回転軸と
回転量で表わすことができる。回転軸Ｊk 、Ｇk 及び回
転量Ｊs 、θは、ここに言う回転軸及び回転量を意味し
ている。基準となる姿勢は、既述のセンサ座標ｘｙｚと
絶対座標ＸＹＺが一致する姿勢である。

【００３０】ジャイロのドリフト量を推定することがで
きれば、回転量Ｊs を略々正確に求めることができるの
で、（３）式、（４）式、及び（５）式における回転量
θについてジャイロのドリフトを補正した回転量Ｊs を
代入することによって、磁気センサ系検出手段２０から
供給されるデータに基づいて、回転軸Ｇk を略々正確に
求めることができる。この場合におけるドリフト量を推
定する演算がドリフト推定演算回路５０によって実行さ
れる。

【００３１】ドリフト補正演算回路６０においては、ド
リフト推定演算回路５０の出力データであるドリフト量
（推定値）と磁気センサ系検出手段２０から供給される
データとに基づいて、誤差が補正された回転量と回転軸
のデータが演算され出力される。

【００３２】上記の角度データ変換回路７０では次のよ
うな演算が行われる。先ず、回転軸を（ｘ，ｙ，ｚ）、
回転量をθとすると、回転行列Ｒk （θ）は次の（６）
式で算出される。

【数６】 ここに行列の各要素を（７）式で表わす。

【数７】 ピッチ（α）、ヨー（β）、ロール（γ）は、COS βに
ついて、次の（８）式で表わされる：

【数８】 そして、α、β、及び、γについては、次の（９）式、
（１０）式、（１１）式で表わされる：

【数９】

【数１０】

【数１１】 この（９）式、（１０）式、（１１）式において、Ａta
n2はArcTan(a/b) を意味する。但し、次の通りａ，ｂの
符号によって各象限に振り分けられる。 ａ＞０， ｂ＞０ のとき第１象限 ａ＜０， ｂ＞０ のとき第２象限 ａ＜０， ｂ＜０ のとき第３象限 ａ＞０， ｂ＜０ のとき第４象限

【００３３】以上説明したように、本実施の形態では、
３個のジャイロセンサによる検出出力に基づいて求めら
れた回転軸と３個の磁気センサによる検出出力に基づい
て求められた回転軸とを比較することによって、ジャイ
ロセンサに係るドリフト量を推定し、この推定値相応し
た補正を行なうことによって、ドリフトが抑制され、応
答性に優れ、分解能が高く、更には、小型軽量なこの種
の３軸姿勢検出装置を実現することができる。

【００３４】ドリフト量の推定については、以下に詳述
する。これには幾つかの方式が考えられる。ジャイロセ
ンサの出力に基づいて得た回転軸Ｊk はドリフトＤの関
数であり、磁気センサの出力に基づいて得た回転軸Ｇk
は回転量θの関数であると考えられる。両者の関数の交
わる範囲が狭ければ相対的にドリフト量の推定の精度は
高くなり比較的正確に回転軸を求めることができる。反
対に両者の関数の交わる範囲が広くなると相対的にドリ
フト量の推定精度は低下し回転軸を求めることは困難に
なる。即ち、回転軸と磁気ベクトルの向きが全く異なる
場合には両者の関数の交わる範囲が狭く、高い精度でド
リフト量の推定を行なうことができる。

【００３５】本発明の装置を前述したＨＭＤに適用する
場合には、一般的に人の頭は磁気ベクトルの回りに回転
することは少ないため、次に説明するようにしてドリフ
トの推定を行なうことができる。尚、ＨＭＤの装着者の
頭部が磁気ベクトルの回りに回転する変位を含んで動い
た場合、この変位が生じている間はドリフトの補正がで
きなくなるが、磁気ベクトルと異なる軸の回りに回転す
る動きに戻ったときに正確な値に補正できるので、磁気
ベクトルの回りに回転する変位が生じている時間が短け
れば実用上問題にはならない。

【００３６】既述の通りジャイロにドリフトがなければ
回転軸Ｇk ＝（Ｇkx，Ｇky，Ｇkz）はジャイロセンサの
出力に基づいて得られる回転軸Ｊk ＝（Ｊkx，Ｊky，Ｊ
kz）と等しくなる。しかし、実際にはジャイロにドリフ
トがあるため、両者は等しくならない。そこで、各ジャ
イロのドリフトをＤx ，Ｄy ，Ｄz とし、上述した式
（１）及び（２）の演算を実行する際にジャイロセンサ
による信号をＪx ＋Ｄx，Ｊy ＋Ｄy ，Ｊz ＋Ｄz とし
て計算を行っておく。

【００３７】ドリフト推定演算回路５０では、評価関数
をＪとして、次の式（１２）の演算を実行する。

【数１２】 そして、この式（１２）においてＪが最小となるＤx ，
Ｄy ，Ｄz を勾配法等の最適化手法を適用して求める。
このようにして求められたＤx ，Ｄy ，Ｄz が各ジャイ
ロのドリフトの推定値となる。

【００３８】また、別の方法として、上述同様にジャイ
ロセンサによる信号をＪx ＋Ｄx ，Ｊy ＋Ｄy ，Ｊz ＋
Ｄz として式（１）及び（２）の演算を実行して、次の
ような式（１３），（１４），（１５）の方程式をたて
る：

【数１３】

【数１４】

【数１５】 ここでＤx ，Ｄy ，Ｄz は非常に小さいと考えられるの
で、Ｄx ，Ｄy ，Ｄzを一次式で近似すると、３元１次
方程式となるので、この方程式を解くことによりＤx ，
Ｄy ，Ｄz を求めることができる。

【００３９】尚、上述したいずれの方法による場合も、
各ジャイロのドリフトを極めて厳密に求めることはでき
ないが、それでも、ジャイロセンサのみによって姿勢検
出を行なうのでは相対値しか測定できないことに起因し
て積み重ねていく程に誤差が集積していってしまうとこ
ろ、本発明によれば相対値でなく絶対値を測定できる磁
気センサによる値を参照するようにしているため、この
ような誤差の集積が生じてしまう虞れが回避される。

【００４０】図３は、本発明の他の実施の形態を示すブ
ロック図である。図３において、既述の図１と同様の機
能部については同一の符号を付して示し、それらの詳細
な説明は省略する。この実施の形態では、磁気ノイズの
多い環境下にも適用可能にするために、磁気センサから
の信号をフィルタリングすべく、磁気センサ系検出手段
２０におけるＡ／Ｄ変換器２７，２８，２９の各次段に
フィルタ回路２７１，２８１，及び２９１が夫々設けら
れている。しかし、フィルタリングするとこれによる時
間遅れが生じてしまうため、ジャイロセンサによる検出
信号とのタイミングにずれを生じることになる。図３の
実施の形態では、この時間遅れを補償するための構成を
備えている。

【００４１】図示の例ではジャイロ回転量・回転軸演算
回路３０内の各入力段に、フィルタ回路２７１，２８
１，及び２９１による遅延時間に相応する遅延特性を得
るための遅延要素（回路）１７１，１８１，及び１９１
が夫々設けられている。尚、これらの遅延要素（回路）
１７１，１８１，及び１９１はジャイロ回転量・回転軸
演算回路３０内に設けるのではなく、その入力側にそれ
ぞれ独立した回路として設けてもよいことは勿論であ
る。

【００４２】一方、上記のようにセンサの出力の伝送路
中に遅延要素が介挿されると、系全体としての応答性が
犠牲になってしまうといった問題がある。本実施の形態
では、この問題に対処すべく、次のような構成をとって
いる。即ち、ジャイロセンサによる検出信号（Ａ／Ｄ変
換後の、ｘｙｚ各軸の回りの回転量のデータ）を遅延要
素１７１，１８１，及び１９１を介することなく、逐
次、データ格納用メモリ１１０にストアする。他方、ド
リフト推定演算回路５０からは、フィルタ回路２７１，
２８１，及び２９１により遅延した磁気センサ系の検出
データ及び遅延要素１７１，１８１，及び１９１による
遅延を被ったジャイロセンサ系の検出データに依拠し
て、既述のようにジャイロセンサ系の出力に関するドリ
フト分のデータ（推定値）が算出される。

【００４３】ドリフト補正演算回路６０では、フィルタ
回路２７１，２８１，及び２９１による遅延時間に相応
する遅延を持った磁気センサの出力に対して上述のよう
にして算出されたドリフト分のデータを考慮し上記のド
リフトによる誤差が補正された回転量のデータ（推定
値）を適用して、回転量・回転軸を算出する。このよう
にして算出された回転量・回転軸が角度データ変換演算
回路７０に供給され、ここで、ピッチ、ロール、ヨーの
各軸に関する回転角のデータとして当該被検出物体の姿
勢が算出される構成は既述の図１の実施の形態と同様で
ある。

【００４４】このようにして算出されたピッチ、ロー
ル、ヨーの各軸に関する回転角のデータは上述の遅延時
間の影響を被っているため、これら各データが得られる
時点を現在時点Ｔとすると、該各回転角のデータによっ
て表わされる事象の現実の発生時点は、現在時点Ｔより
も時間Δｔだけ遡及した時点である。即ち、角度データ
変換演算回路７０の出力は、時間Δｔだけ以前の事象
（状態）を表わしている。そこで、上述のように遅延要
素による遅延を受けることなくデータ格納用メモリ１１
０にストアされているデータを利用して現在時点Ｔにお
けるｘｙｚ各軸の回りの回転量のデータを推定する。

【００４５】この様子を図４のタイミング図を併せ参照
して説明する。尚、ｘｙｚ各軸に係る各磁気センサ２
３，２４，２５の出力（フィルタ回路２７１，２８１，
及び２９１による遅延を受けた出力）は同時に得られる
ものとし、同様にｘｙｚ各軸に係る各ジャイロセンサ１
１，１２，１３の出力も同時に得られるものとしてある
ため、この図４では上記各磁気センサ２３，２４，２５
の出力を総じて「磁気センサ系からのデータ」、同様に
各ジャイロセンサ１１，１２，１３の出力を総じて「ジ
ャイロセンサ系からのデータ」と表記してある。

【００４６】磁気センサ系からの出力はフィルタ回路２
７１，２８１，及び２９１による遅延時間Δｔがあるた
め、図４に示されたように磁気センサ系から現在時点Ｔ
においてデータＧ４が得られるとき、この磁気センサ系
からのデータＧ４と同時刻における現象を表わすジャイ
ロセンサ系からのデータは、時点Ｔより時間Δｔだけ過
去に遡及した時点で得られたデータＪ１である。このた
め、当該現象（被検出物体の姿勢）が生起した時刻に関
する整合性が計られた両データである上記磁気センサ系
からのデータＧ４とジャイロセンサ系からのデータＪ１
とを用いて、図３について上述したように、現在時点Ｔ
よりも時間Δｔだけ遡及した時点におけるピッチ、ロー
ル、ヨーの各軸に関する回転角のデータとして当該被検
出物体の姿勢が算出される。

【００４７】ここで、既述のように、データ格納用メモ
リには時点Ｔより過去のジャイロセンサ出力に依るデー
タがストアされているので、過去の時間Δｔの期間に亘
るジャイロセンサ出力に依るデータ（ジャイロセンサ系
からのデータＪ１，Ｊ２，Ｊ３）を用いてこの時間Δｔ
の間にピッチ、ロール、ヨーの各軸に関する回転角が如
何ほど変化したかをジャイロ回転角演算回路１２０で算
出する。このようにしてジャイロ回転角演算回路１２０
で算出されたピッチ、ロール、ヨーの各軸に関する回転
角のΔｔ間での変化量のデータを用いて、時間軸シフト
演算回路９０において、角度データ変換演算回路７０に
て先に得られた回転角のデータに対して補正をかけるこ
とにより、上述の遅延時間の影響が十分に補償されたデ
ータが得られる。

【００４８】尚、図３の実施の形態では、ジャイロ回転
角演算回路１２０で算出されるデータはジャイロ系の出
力のみに依拠したものであるため、理論上はジャイロの
ドリフトが含まれることになるが、現実には、上述の時
間Δｔは高々１秒以下の値であるため、実用上は何等支
障がなく無視できる程度のものである。本実施の形態に
よれば、上述のようにして、磁気センサ系に夫々フィル
タ回路２７１，２８１，及び２９１を介挿させたことに
よる遅延時間の影響を最小限のものとすることによっ
て、磁気ノイズの比較的多い環境下においてもノイズの
影響が除去され且つ応答性にも優れた姿勢検出装置を実
現することができる。

【００４９】図５は本発明の更に他の実施の形態を示す
ブロック図である。この実施の形態はワンチップマイク
ロコンピュータを用いて装置を構成した例である。この
図５においても、ジャイロセンサ系検出手段及び磁気セ
ンサ系検出手段等既述の図１及び図３と同様の機能部に
ついては同一の符号を付して示し、それらの詳細な説明
は省略する。Ａ／Ｄ変換器の機能部はＡ／Ｄコンバータ
入力ポート３１０として、通信インタフェース回路の機
能部は通信インタフェースポート３２０として、このワ
ンチップマイクロコンピュータ３００に備えられてい
る。尚、これら機能部をワンチップマイクロコンピュー
タに備えず、所謂外付けの構成を採ることもできる。

【００５０】図５の実施の形態では、図３の実施の形態
におけるジャイロセンサ系検出手段及び磁気センサ系検
出手段の後段の各回路ブロックに相当する機能はワンチ
ップマイクロコンピュータ３００によって実現される。

【００５１】図６はこのような機能を果たすためのワン
チップマイクロコンピュータ３００の動作を表わすフロ
ーチャートである。ステップＳ１でジャイロセンサ系検
出手段及び磁気センサ系検出手段の出力データを夫々Ａ
／Ｄコンバータ入力ポート３１０を介して読み込む。次
にステップＳ２でジャイロ系検出手段の出力データに基
づいて回転軸と回転量を算出する。また、ステップＳ３
で磁気センサ系検出手段の出力データに基づいて回転軸
を算出する。次にステップＳ４でジャイロセンサ系検出
手段及び磁気センサ系検出手段の両出力データの比較に
基づいて、算出の手法については図１及び図３の実施の
形態におけると同様にして、ドリフト量を推定するため
の演算を実行する。

【００５２】ステップＳ５では、このようにして推定さ
れたドリフト量の値を用いて図１及び図３の実施の形態
のドリフト補正演算回路６０におけると同様にしてこの
ドリフト量に関する補正を実行する。そしてステップＳ
６では誤差が補正された回転軸・回転量の表現による検
出出力をピッチ軸、ロール軸及びヨー軸回りの回動角に
よる表現に変換する。

【００５３】次に、ステップＳ７では、一旦メモリに格
納しておいたジャイロセンサ系検出手段の出力データを
利用して図３の実施の形態の時間軸シフト演算回路９０
におけると同様にして時間軸をシフトする。次のステッ
プＳ８で、通信インタフェースポート３２０を介して出
力データをシリアル又はパラレルの形態で適宜の映像信
号処理装置等の外部のシステム（例えば、ホスト側コン
ピュータ）に伝送して、システムとしての所定の制御や
信号処理動作等が行なわれ得る。以上のようなワンチッ
プマイクロコンピュータを用いて構成した実施の形態で
は、部品点数の少ない簡素な構成の３軸姿勢検出装置が
実現される。

【００５４】尚、上述した各実施の形態では、姿勢表現
を回転軸・回転量表現からピッチ軸、ロール軸及びヨー
軸回りの回動角による表現に変換して出力するように構
成しているが、回転軸・回転量表現のまま出力するよう
に構成してもよいし、或いはまた、他の姿勢表現に変換
して出力するように構成してもよい。

【００５５】また、上述における各演算の計算式は、外
部座標のＸ軸の方向が地磁気ベクトルの方向と一致する
ようにした場合を前提としている。このため、磁気セン
サが地磁気ベクトルの方向を向いているときが基準とな
るので、実際には、地平面と平行でないときにピッチ及
びロール軸回りの回動角が０になる。アプリケーション
によっては、地平面がＸＹ平面となるように座標変換を
行なう必要があるので、所要に応じてこの座標変換を行
なえばよい。

【００５６】

【発明の効果】本願発明によれば、ドリフト等に起因す
る検出誤差が少なく、且つ構成が簡単なこの種の３軸姿
勢検出装置を提供することができる。即ち、請求項１の
発明によれば、誤差の少ない３軸姿勢検出装置が実現さ
れ、且つ、構成の簡素化とコストの低減も図られる。ま
た、請求項２の発明によれば、誤差の少ない３軸姿勢検
出装置が実現され、且つ、構成の簡素化とコスト低減も
図られる。特にこの発明では、ジャイロセンサによるド
リフトの影響が確実に補正された出力が得られる。請求
項３の発明によれば、誤差の少ない３軸姿勢検出装置が
実現され、且つ、構成の簡素化とコスト低減も図られ
る。特にこの発明では、磁気センサ系での応答遅れによ
る遅延時間の影響を、時間軸の遡及的シフト効果を奏す
る補正をかけて最小限のものとすることによって、磁気
ノイズの比較的多い環境下において磁気センサ系にフィ
ルタ等を介挿させた場合等においても応答性に優れた姿
勢検出装置を実現することができる。更に、請求項４の
発明によれば、ジャイロセンサの出力を回転量とこれに
係る回転軸を表わすデータに変換し、磁気センサの出力
を回転軸を表わすデータに変換してから、検出誤差（ジ
ャイロセンサに関するドリフト）の推定を行うことがで
きるため、このための演算が簡素化される。

【図面の簡単な説明】

【図１】本願発明の一つの実施の形態としての３軸姿勢
検出装置を示すブロック図である。

【図２】図１における第１乃至第３のジャイロセンサ及
び、第１乃至第３の磁気センサの配置と、目的物体と一
緒に動く相互に直交する３軸（ｘ，ｙ，ｚ：以下センサ
座標ｘｙｚという）、並びに、基準となる動かない直交
座標系である外部座標ＸＹＺとの関係を示す模式図であ
る。

【図３】本願発明の他の実施の形態としての３軸姿勢検
出装置を示すブロック図である。

【図４】図３の実施の形態の動作を説明するためのタイ
ミング図である。

【図５】本発明の更に他の実施の形態を示すブロック図
である。

【図６】図５の実施の形態におけるワンチップマイクロ
コンピュータの動作を説明するためのフローチャートで
ある。

【符号の説明】

１０ ジャイロセンサ系検出手段 １１ 第１のジャイロセンサ １２ 第２のジャイロセンサ １３ 第３のジャイロセンサ １４〜１６ 増幅器 １７〜１９ Ａ／Ｄ変換器 ２０ 磁気センサ系検出手段 ２１ 第１の磁気センサ ２２ 第２の磁気センサ ２３ 第３の磁気センサ ２４〜２６ 増幅器 ２７〜２９ Ａ／Ｄ変換器 ３０ ジャイロ回転量・回転軸演算
回路 ４０ 磁気センサ回転軸演算回路 ５０ ドリフト推定演算回路 ６０ ドリフト補正演算回路 ７０ 角度データ変換回路 ８０ 通信インタフェース回路 ９０ 時間軸シフト演算回路 １１０ データ格納用メモリ １２０ ジャイロ回転角演算回路 １７１，１８１，１９１ 遅延要素（回路） ２７１，２８１，２９１ フィルタ回路 ３００ ワンチップマイクロコンピュ
ータ ３１０ Ａ／Ｄコンバータ入力ポート ３２０ 通信インタフェースポート

Claims (4)

【特許請求の範囲】
1. 【請求項１】目的物体の空間内での姿勢を検出するため
   に、該目的物体に関して仮定された相互に交差する３軸
   の各軸の回りの回動角について、該回動角の検出の基礎
   となる角速度を一次的に検出しこの一次的検出値の経時
   的積分値である二次的検出値としてこれら回動角を検出
   すべく該３軸のうちの各該当する軸に対応して設けられ
   た第１のジャイロセンサ、第２のジャイロセンサ、及び
   第３のジャイロセンサと、 上記３軸の各軸の回りの回動角について、該回動角の検
   出の基礎となる方位角を検出すべく該３軸のうちの各該
   当する軸に対応して設けられた第１の磁気センサ、第２
   の磁気センサ、及び第３の磁気センサと、 上記第１乃至第３の磁気センサによる検出値に対し上記
   第１乃至第３のジャイロセンサによる検出値に依拠して
   得たデータを所定の演算により適用して誤差が補正され
   た３軸姿勢検出出力を得る演算手段と、を備えてなるこ
   とを特徴とする３軸姿勢検出装置。
2. 【請求項２】目的物体の空間内での姿勢を検出するため
   に、該目的物体に関して仮定された相互に交差する３軸
   の各軸の回りの回動角について、該回動角の検出の基礎
   となる角速度を一次的に検出しこの一次的検出値の経時
   的積分値である二次的検出値としてこれら回動角を検出
   すべく該３軸のうちの各該当する軸に対応して設けられ
   た第１のジャイロセンサ、第２のジャイロセンサ、及び
   第３のジャイロセンサと、 上記３軸の各軸の回りの回動角について、該回動角の検
   出の基礎となる方位角を検出すべく該３軸のうちの各該
   当する軸に対応して設けられた第１の磁気センサ、第２
   の磁気センサ、及び第３の磁気センサと、 上記第１乃至第３のジャイロセンサによる回動角の検出
   値および上記第１乃至第３の磁気センサによる方位角の
   検出値に基づいて、上記第１乃至第３のジャイロセンサ
   の出力たる上記二次的検出値としての回動角の検出値に
   含まれる上記一次的検出値の誤差分に関する経時的積分
   値であるドリフト成分の値を推定するドリフト推定演算
   手段と、 上記第１乃至第３の磁気センサによる検出値に対し上記
   ドリフト推定演算手段により算出されたドリフト成分の
   推定値を適用して誤差が補正された３軸姿勢検出出力を
   得るドリフト補正演算手段と、を備えてなることを特徴
   とする３軸姿勢検出装置。
3. 【請求項３】目的物体の空間内での姿勢を検出するため
   に、該目的物体に関して仮定された相互に交差する３軸
   の各軸の回りの回動角について、該回動角の検出の基礎
   となる角速度を一次的に検出しこの一次的検出値の経時
   的積分値である二次的検出値としてこれら回動角を検出
   すべく該３軸のうちの各該当する軸に対応して設けられ
   た第１のジャイロセンサ、第２のジャイロセンサ、及び
   第３のジャイロセンサを含んで構成されたジャイロセン
   サ系検出手段と、 上記３軸の各軸の回りの回動角について、該回動角の検
   出の基礎となる方位角を検出すべく該３軸のうちの各該
   当する軸に対応して設けられた第１の磁気センサ、第２
   の磁気センサ、及び第３の磁気センサを含んで構成され
   た磁気センサ系検出手段と、 上記ジャイロセンサ系検出手段による回動角の検出値た
   るジャイロセンサ系検出データおよび上記磁気センサ系
   検出手段による方位角の検出値たる磁気センサ系検出デ
   ータを、これらジャイロセンサ系検出データの表わす事
   象と磁気センサ系検出データの表わす事象との両事象の
   時間的同時性が実効的に計られるように適用して、上記
   ジャイロセンサ系検出データたる上記二次的検出値とし
   ての回動角の検出値に含まれる上記一次的検出値の誤差
   に関する経時的積分値であるドリフト成分の値を推定す
   るドリフト推定演算手段と、 上記磁気センサ系検出データに対し上記ドリフト推定演
   算手段により算出されたドリフト成分の推定値を適用し
   て誤差が補正された３軸姿勢検出出力を得るドリフト補
   正演算手段と、 上記ジャイロセンサ系検出手段に対する磁気センサ系検
   出手段の検出の応答遅れに相応する時間以上の時間区間
   に亘って実効的に遅延が与えられていない状態での上記
   ジャイロセンサ系検出データを蓄積可能なデータ格納用
   メモリと、 上記データ格納用メモリに蓄積されたジャイロセンサ系
   検出データに依拠して上記磁気センサ系検出手段の当該
   応答遅れに相応する時間内での上記３軸姿勢検出出力の
   変化量の予測値を算出する変化量予測演算手段と、 上記ドリフト補正演算手段の出力たる３軸姿勢検出出力
   に上記変化量予測演算手段で算出された３軸姿勢検出出
   力の変化量の予測値を加味して実効的に時間軸を遡及し
   たと等価な３軸姿勢検出出力を得る時間軸シフト演算手
   段と、備えてなることを特徴とする３軸姿勢検出装置。
4. 【請求項４】上記第１のジャイロセンサ、第２のジャイ
   ロセンサ、及び第３のジャイロセンサの上記一次的検出
   値たる上記３軸の各軸の回りの角速度のデータに基づい
   て上記二次的検出値たる回動角を回転量とこれに係る回
   転軸を表わすデータとして出力するように構成されたジ
   ャイロ回転量・回転角演算手段と、 上記第１の磁気センサ、第２の磁気センサ、及び第３の
   磁気センサの出力に基づいて回転軸を表わすデータを出
   力するように構成された磁気センサ回転軸変換手段と、
   を備えたことを特徴とする請求項１，２または３に記載
   の３軸姿勢検出装置。