JPH11248456A - 3軸姿勢検出装置 - Google Patents
3軸姿勢検出装置Info
- Publication number
- JPH11248456A JPH11248456A JP6444198A JP6444198A JPH11248456A JP H11248456 A JPH11248456 A JP H11248456A JP 6444198 A JP6444198 A JP 6444198A JP 6444198 A JP6444198 A JP 6444198A JP H11248456 A JPH11248456 A JP H11248456A
- Authority
- JP
- Japan
- Prior art keywords
- detection
- value
- axis
- magnetic sensor
- gyro sensor
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Landscapes
- Gyroscopes (AREA)
- Measuring Magnetic Variables (AREA)
Abstract
機、その他の種々の目的物体の空間内での姿勢を、ピッ
チ軸、ロール軸及びヨー軸等の該目的物体に関して仮定
された相互に交差する3軸の各軸の回りの回動角として
検出する3軸姿勢検出装置の構成の簡素化を計り、且
つ、検出精度を向上させる。 【解決手段】第1乃至第3のジャイロセンサ(11〜1
3)による回動角の検出値および第1乃至第3の磁気セ
ンサ(21〜13)による方位角の検出値に基づいて、
第1乃至第3のジャイロセンサ(11〜13)の出力た
る上記二次的検出値としての回動角の検出値に含まれる
一次的検出値の誤差分に関する経時的積分値であるドリ
フト成分の値をドリフト推定演算回路(50)で推定
し、ドリフト補正演算回路(60)で第1乃至第3の磁
気センサ(21〜13)による検出値に対しドリフト成
分の推定値を適用して誤差が補正された3軸姿勢検出出
力を得る
Description
型のディスプレイ装置、或いは、航空機、その他の種々
の目的物体の空間内での姿勢を、ピッチ軸、ロール軸及
びヨー軸等の該目的物体に関して仮定された相互に交差
する3軸の各軸の回りの回動角として検出する3軸姿勢
検出装置に関する。
Mounted Display:HMDと略称される)に姿勢検出器
を備えることについては、例えば、米国特許第5,373,85
7 号明細書、特開平3-289615号公報等に開示がある。
は、ジャイロを上記3軸の各軸に対応させて3個直交す
るように配置し、ピッチ軸、ロール軸及びヨー軸の回り
の回動角を検出するものがあった。また、同様の方式の
3軸姿勢検出装置で、頭部装着型のディスプレイ装置の
姿勢検出にも適合する小型のシステムとしては、日本航
空電子株式会社製のJMIS30Sマックスキューブ
(商標)という検出器の応用技術として既に公知のもの
がある。
傾斜センサと磁気センサとを併用したシステムもある。
例えば、米国プレシジョンナビゲーション社のシステム
のようにいずれも液体式(密閉容器内の液体の液面の動
きで傾きを検出する方式)の2軸の傾斜センサと磁気セ
ンサ3個を併用し、傾斜センサでピッチ軸及びロール軸
回りの回動角を検出し、磁気センサでヨー軸の回りの回
動角を検出するものである。尚、株式会社ワコーから
は、半導体型3軸加速度センサPRGシリーズ(商標)
という、非液体式の検出器が提供され、このセンサを傾
斜センサとして用いる方式も考えられる。
動ジャイロ3個と加速度センサを3個(3軸)用いて頭
部装着型のディスプレイ装置の姿勢検出に適合する小型
のシステムが提供されている。
びヨー軸の回りの回動角を検出するように3個直交する
ように配置する方式のものでは、ジャイロセンサ自体が
回動角の検出の基礎となる角速度を一次的に検出しこの
一次的検出値の経時的積分値である二次的検出値として
これら回動角を検出するものであるため、次のような問
題がある。即ち、一次的検出対象である角速度の真値が
0であるときに、センサ出力としてこれからずれた値を
示すような特性を呈するとき、換言すれば、検出データ
にオフセットが含まれるときには、これの経時的積分値
である二次的検出値としての回動角のデータは、時間の
経過と共に益々その誤差の幅が拡大されてしまうことに
なる。即ち、ジャイロセンサによる場合は、回動角のデ
ータにドリフトが生じてしまい、これによる誤差は時間
の経過と共に拡大してしまうことになる。
を併用したシステムでは、既述のように、傾斜センサに
よってピッチ軸及びロール軸回りの回動角を検出し、磁
気センサによってヨー軸の回りの回動角を検出する。こ
の場合、傾斜センサは重力加速度を基準としているた
め、平行移動による誤差が生じる。また、液体式の傾斜
センサは密閉容器内の液体の液面の傾きに依拠して検出
値を得るという構造上、加速度が作用している間は検出
誤差を生じる(慣性力による誤差)。このため、速度が
定常となるとき、即ち、この加速度が0となるときを待
って検出値を得る必要があることから、検出に遅れが生
じてしまう。尚、非液体式の傾斜センサを適用する方式
を採った場合も、平行移動による誤差は不可避である。
3個(3軸)用いる方式では、加速度センサを3個(3
軸)用いて傾斜センサを構成し、ジャイロのドリフトを
補正するようにしている。しかしながら、傾斜センサに
よる場合、ピッチ軸及びロール軸回りの回動角しか検出
できないため、ヨー軸の回りの回動角に関するドリフト
を補正することができない。これを補正するためには、
更に磁気センサを3個付加してヨー軸に係るドリフトを
補正することも考えられるが、センサの数が極めて多く
なり、システムが複雑化し、大型化が避けられず、コス
トも高くなってしまう。
な事情に鑑みてなされたものであり、上述のドリフト等
に起因する検出誤差が少なく、且つ構成が簡単なこの種
の3軸姿勢検出装置を提供しようとするものである。
決するための、一つの本願発明は:目的物体の空間内で
の姿勢を検出するために、該目的物体に関して仮定され
た相互に交差する3軸の各軸の回りの回動角について、
該回動角の検出の基礎となる角速度を一次的に検出しこ
の一次的検出値の経時的積分値である二次的検出値とし
てこれら回動角を検出すべく該3軸のうちの各該当する
軸に対応して設けられた第1のジャイロセンサ、第2の
ジャイロセンサ、及び第3のジャイロセンサと、上記3
軸の各軸の回りの回動角について、該回動角の検出の基
礎となる方位角を検出すべく該3軸のうちの各該当する
軸に対応して設けられた第1の磁気センサ、第2の磁気
センサ、及び第3の磁気センサと、上記第1乃至第3の
磁気センサによる検出値に対し上記第1乃至第3のジャ
イロセンサによる検出値に依拠して得たデータを所定の
演算により適用して誤差が補正された3軸姿勢検出出力
を得る演算手段と、を備えてなることを特徴とする3軸
姿勢検出装置というものである……[1]
3軸姿勢検出装置が実現され、且つ、構成の簡素化とコ
ストの低減も図られる。
空間内での姿勢を検出するために、該目的物体に関して
仮定された相互に交差する3軸の各軸の回りの回動角に
ついて、該回動角の検出の基礎となる角速度を一次的に
検出しこの一次的検出値の経時的積分値である二次的検
出値としてこれら回動角を検出すべく該3軸のうちの各
該当する軸に対応して設けられた第1のジャイロセン
サ、第2のジャイロセンサ、及び第3のジャイロセンサ
と、上記3軸の各軸の回りの回動角について、該回動角
の検出の基礎となる方位角を検出すべく該3軸のうちの
各該当する軸に対応して設けられた第1の磁気センサ、
第2の磁気センサ、及び第3の磁気センサと、上記第1
乃至第3のジャイロセンサによる回動角の検出値および
上記第1乃至第3の磁気センサによる方位角の検出値に
基づいて、上記第1乃至第3のジャイロセンサの出力た
る上記二次的検出値としての回動角の検出値に含まれる
上記一次的検出値の誤差分に関する経時的積分値である
ドリフト成分の値を推定するドリフト推定演算手段と、
上記第1乃至第3の磁気センサによる検出値に対し上記
ドリフト推定演算手段により算出されたドリフト成分の
推定値を適用して誤差が補正された3軸姿勢検出出力を
得るドリフト補正演算手段と、を備えてなることを特徴
とする3軸姿勢検出装置というものである……[2]
3軸姿勢検出装置が実現され、且つ、構成の簡素化とコ
スト低減も図られる。特にこの発明では、ジャイロセン
サによるドリフトの影響が確実に補正された出力が得ら
れる。
物体の空間内での姿勢を検出するために、該目的物体に
関して仮定された相互に交差する3軸の各軸の回りの回
動角について、該回動角の検出の基礎となる角速度を一
次的に検出しこの一次的検出値の経時的積分値である二
次的検出値としてこれら回動角を検出すべく該3軸のう
ちの各該当する軸に対応して設けられた第1のジャイロ
センサ、第2のジャイロセンサ、及び第3のジャイロセ
ンサを含んで構成されたジャイロセンサ系検出手段と、
上記3軸の各軸の回りの回動角について、該回動角の検
出の基礎となる方位角を検出すべく該3軸のうちの各該
当する軸に対応して設けられた第1の磁気センサ、第2
の磁気センサ、及び第3の磁気センサを含んで構成され
た磁気センサ系検出手段と、上記ジャイロセンサ系検出
手段による回動角の検出値たるジャイロセンサ系検出デ
ータおよび上記磁気センサ系検出手段による方位角の検
出値たる磁気センサ系検出データを、これらジャイロセ
ンサ系検出データの表わす事象と磁気センサ系検出デー
タの表わす事象との両事象の時間的同時性が実効的に計
られるように適用して、上記ジャイロセンサ系検出デー
タたる上記二次的検出値としての回動角の検出値に含ま
れる上記一次的検出値の誤差に関する経時的積分値であ
るドリフト成分の値を推定するドリフト推定演算手段
と、上記磁気センサ系検出データに対し上記ドリフト推
定演算手段により算出されたドリフト成分の推定値を適
用して誤差が補正された3軸姿勢検出出力を得るドリフ
ト補正演算手段と、上記ジャイロセンサ系検出手段に対
する磁気センサ系検出手段の検出の応答遅れに相応する
時間以上の時間区間に亘って実効的に遅延が与えられて
いない状態での上記ジャイロセンサ系検出データを蓄積
可能なデータ格納用メモリと、上記データ格納用メモリ
に蓄積されたジャイロセンサ系検出データに依拠して上
記磁気センサ系検出手段の当該応答遅れに相応する時間
内での上記3軸姿勢検出出力の変化量の予測値を算出す
る変化量予測演算手段と、上記ドリフト補正演算手段の
出力たる3軸姿勢検出出力に上記変化量予測演算手段で
算出された3軸姿勢検出出力の変化量の予測値を加味し
て実効的に時間軸を遡及したと等価な3軸姿勢検出出力
を得る時間軸シフト演算手段と、備えてなることを特徴
とする3軸姿勢検出装置というものである………[3]
3軸姿勢検出装置が実現され、且つ、構成の簡素化とコ
スト低減も図られる。特にこの発明では、磁気センサ系
での応答遅れによる遅延時間の影響を、時間軸の遡及的
シフト効果を奏する補正をかけて最小限のものとするこ
とによって、磁気ノイズの比較的多い環境下において磁
気センサ系にフィルタ等を介挿させた場合等においても
応答性に優れた姿勢検出装置を実現することができる。
ジャイロセンサ、第2のジャイロセンサ、及び第3のジ
ャイロセンサの上記一次的検出値たる上記3軸の各軸の
回りの角速度のデータに基づいて上記二次的検出値たる
回動角を回転量とこれに係る回転軸を表わすデータとし
て出力するように構成されたジャイロ回転量・回転角演
算手段と、上記第1の磁気センサ、第2の磁気センサ、
及び第3の磁気センサの出力に基づいて回転軸を表わす
データを出力するように構成された磁気センサ回転軸変
換手段と、を備えたことを特徴とする上記[1],
[2]または[3]に記載の3軸姿勢検出装置というも
のである………………………………………………………
[4]
サの出力を回転量とこれに係る回転軸を表わすデータに
変換し、磁気センサの出力を回転軸を表わすデータに変
換してから、検出誤差(ジャイロセンサに関するドリフ
ト)の推定を行うことができるため、このための演算が
簡素化される。
施の形態について詳述することにより本発明を明らかに
する。
の3軸姿勢検出装置を示すブロック図である。図1にお
いて、例えば、航空機、工業用ロボット、または、HM
Dのような目的物体の空間内での姿勢を検出するため
に、該目的物体に関して仮定された相互に交差する3軸
(x,y,z)の各軸の回りの回動角について、該回動
角の検出の基礎となる角速度を一次的に検出しこの一次
的検出値の経時的積分値である二次的検出値としてこれ
ら回動角を検出すべく該3軸(x,y,z)のうちx軸
に対応して第1のジャイロセンサ11が設けられ、y軸
に対応して第2のジャイロセンサ12が設けられ、z軸
に対応して第3のジャイロセンサ13が設けられてい
る。
りの回動角について、該回動角の検出の基礎となる方位
角を検出すべく該3軸のうちx軸に対応して第1の磁気
センサ21が設けられ、y軸に対応して第2の磁気セン
サ22が設けられ、z軸に対応して第3の磁気センサ2
3が設けられている。
サ11,12,13、及び、第1乃至第3の磁気センサ
21,22,23の配置と、上記目的物体と一緒に動く
相互に直交する3軸(x,y,z:以下センサ座標xy
zという)、並びに、基準となる動かない直交座標系で
ある外部座標XYZとの関係を示す模式図である。基準
となる外部座標XYZは演算を簡素化するために、X軸
方向が地磁気ベクトルの方向と一致するように設定され
ている。このセンサ座標xyz及び外部座標XYZとの
関係において上記の第1乃至第3の磁気センサ21,2
2,23による検出データをより厳密に言えば次の通り
である。即ち、第1の磁気センサ21はセンサ座標xy
zのx軸に地磁気ベクトルを投影した値を検出データと
して出力し、第2の磁気センサ22はセンサ座標xyz
のy軸に地磁気ベクトルを投影した値を検出データとし
て出力し、第3の磁気センサ23はセンサ座標xyzの
z軸に地磁気ベクトルを投影した値を検出データとして
出力する。
12,13は上記各回動角の検出値を得るための基礎と
なる3軸(x,y,z)の各軸の回りの角速度の検出デ
ータを一次的に生成する。図示の実施の形態では、これ
ら一次的検出データである各角速度の検出データは、第
1乃至第3のジャイロセンサ11,12,13の内部の
回路でそれらデータの各経時的積分値を得るための演算
に付され、センサ座標xyzの各軸の回りの回動角を表
わす二次的検出値のデータJx ,Jy ,Jz として出力
される。これらの出力データJx ,Jy ,Jz は、夫々
に対応する増幅器14,15,16、並びに、これらの
増幅器14,15,16の各後段に設けられたA/D変
換器17,18,19を介して、ディジタルデータに変
換され、ジャイロ回転量・回転軸演算回路30に夫々供
給される。ジャイロ回転量・回転軸演算回路30は、上
記供給されたデータに基づいて回転量とこの回転量に係
る回転軸を求める演算を実行する。尚、上記第1乃至第
3のジャイロセンサ11,12,13、増幅器14,1
5,16及びA/D変換器17,18,19はジャイロ
センサ系検出手段10を構成する。
1,22,23による方位角の各検出値GX ,Gy ,G
z は、各対応する増幅器24,25,26、並びに、こ
れらの増幅器24,25,26の各後段に設けられたA
/D変換器27,28,29を介して、ディジタルデー
タに変換され、磁気センサ回転量演算回路40に夫々供
給される。尚、上記第1乃至第3の磁気センサ21,2
2,23、増幅器24,25,26及びA/D変換器2
7,28,29は磁気センサ系検出手段20を構成す
る。
センサ系検出手段20から供給されるデータに基づい
て、回転軸を求めるための演算を実行する。この磁気セ
ンサ回転軸演算回路40の出力及び上記ジャイロ回転量
・回転軸演算回路30の出力は、これらの出力に基づい
て(結局、上記出力データJx ,Jy ,Jz 及びGX ,
Gy ,Gz に基づいて)、上記第1乃至第3のジャイロ
センサの出力たる上記二次的検出値としての回動角の検
出値(センサ座標xyzの各軸の回りの回動角を表わす
二次的検出値のデータJx ,Jy ,Jz )に含まれる上
記一次的検出値(角速度検出値)の誤差分に関する経時
的積分値であるドリフト成分の値を推定するドリフト推
定演算回路50に供給される。尚、ドリフトの推定につ
いては後に詳述する。
8,29の出力はドリフト補正演算回路60にも供給さ
れる。このドリフト補正演算回路60にはまた、上記ド
リフト推定演算回路50の出力も供給される。ドリフト
補正演算回路60は、第1乃至第3の磁気センサによる
検出値GX ,Gy ,Gz に対し上記ドリフト推定演算手
段50により算出されたドリフト成分の推定値を適用し
て誤差が補正された3軸姿勢検出出力を得る。
補正された回転軸・回転量の表現による姿勢検出出力
は、次段の角度データ変換回路70に供給され、ここで
ピッチ軸、ロール軸及びヨー軸回りの回動角による表現
に変換され、更に次段の通信インタフェース回路80を
介して適宜の映像信号処理装置等の外部のシステム(本
例のシステムの場合はホスト側コンピュータ)に伝送さ
れ、所定の制御や信号処理動作等を行なうに供される。
演算について、次に数式を用いて説明する。既述のよう
に、ジャイロ回転量・回転軸演算回路30ではジャイロ
センサ系検出手段10から供給されるデータに基づいて
回転量とこの回転量に係る回転軸を求める演算を実行す
る。この演算のうち、上記の回転量Js を求めるための
演算は次の(1)式に基づき実行される。
演算は次の(2)式に基づき実行される。
いて、磁気センサ系検出手段20から供給されるデータ
に基づいて、回転軸Gk を求めるための演算は次のよう
にして行われる。先ず、回転軸ベクトルGk をGk =
(Gkx,Gky,Gkz)として表わすと、Gkx、Gky、及
び、Gkzは、夫々次の(3)式、(4)式、及び(5)
式のようにして算出される。
はできないため、上記のように回転軸ベクトルGk をθ
の関数として表している。ジャイロにドリフトがなけれ
ば、θにJs を代入すれば、Gk とJk は等しくなる。
ある基準の姿勢から、現在の姿勢への変化を、回転軸と
回転量で表わすことができる。回転軸Jk 、Gk 及び回
転量Js 、θは、ここに言う回転軸及び回転量を意味し
ている。基準となる姿勢は、既述のセンサ座標xyzと
絶対座標XYZが一致する姿勢である。
きれば、回転量Js を略々正確に求めることができるの
で、(3)式、(4)式、及び(5)式における回転量
θについてジャイロのドリフトを補正した回転量Js を
代入することによって、磁気センサ系検出手段20から
供給されるデータに基づいて、回転軸Gk を略々正確に
求めることができる。この場合におけるドリフト量を推
定する演算がドリフト推定演算回路50によって実行さ
れる。
リフト推定演算回路50の出力データであるドリフト量
(推定値)と磁気センサ系検出手段20から供給される
データとに基づいて、誤差が補正された回転量と回転軸
のデータが演算され出力される。
うな演算が行われる。先ず、回転軸を(x,y,z)、
回転量をθとすると、回転行列Rk (θ)は次の(6)
式で算出される。
ついて、次の(8)式で表わされる:
(10)式、(11)式で表わされる:
n2はArcTan(a/b) を意味する。但し、次の通りa,bの
符号によって各象限に振り分けられる。 a>0, b>0 のとき第1象限 a<0, b>0 のとき第2象限 a<0, b<0 のとき第3象限 a>0, b<0 のとき第4象限
3個のジャイロセンサによる検出出力に基づいて求めら
れた回転軸と3個の磁気センサによる検出出力に基づい
て求められた回転軸とを比較することによって、ジャイ
ロセンサに係るドリフト量を推定し、この推定値相応し
た補正を行なうことによって、ドリフトが抑制され、応
答性に優れ、分解能が高く、更には、小型軽量なこの種
の3軸姿勢検出装置を実現することができる。
する。これには幾つかの方式が考えられる。ジャイロセ
ンサの出力に基づいて得た回転軸Jk はドリフトDの関
数であり、磁気センサの出力に基づいて得た回転軸Gk
は回転量θの関数であると考えられる。両者の関数の交
わる範囲が狭ければ相対的にドリフト量の推定の精度は
高くなり比較的正確に回転軸を求めることができる。反
対に両者の関数の交わる範囲が広くなると相対的にドリ
フト量の推定精度は低下し回転軸を求めることは困難に
なる。即ち、回転軸と磁気ベクトルの向きが全く異なる
場合には両者の関数の交わる範囲が狭く、高い精度でド
リフト量の推定を行なうことができる。
場合には、一般的に人の頭は磁気ベクトルの回りに回転
することは少ないため、次に説明するようにしてドリフ
トの推定を行なうことができる。尚、HMDの装着者の
頭部が磁気ベクトルの回りに回転する変位を含んで動い
た場合、この変位が生じている間はドリフトの補正がで
きなくなるが、磁気ベクトルと異なる軸の回りに回転す
る動きに戻ったときに正確な値に補正できるので、磁気
ベクトルの回りに回転する変位が生じている時間が短け
れば実用上問題にはならない。
回転軸Gk =(Gkx,Gky,Gkz)はジャイロセンサの
出力に基づいて得られる回転軸Jk =(Jkx,Jky,J
kz)と等しくなる。しかし、実際にはジャイロにドリフ
トがあるため、両者は等しくならない。そこで、各ジャ
イロのドリフトをDx ,Dy ,Dz とし、上述した式
(1)及び(2)の演算を実行する際にジャイロセンサ
による信号をJx +Dx,Jy +Dy ,Jz +Dz とし
て計算を行っておく。
をJとして、次の式(12)の演算を実行する。
Dy ,Dz を勾配法等の最適化手法を適用して求める。
このようにして求められたDx ,Dy ,Dz が各ジャイ
ロのドリフトの推定値となる。
ロセンサによる信号をJx +Dx ,Jy +Dy ,Jz +
Dz として式(1)及び(2)の演算を実行して、次の
ような式(13),(14),(15)の方程式をたて
る:
で、Dx ,Dy ,Dzを一次式で近似すると、3元1次
方程式となるので、この方程式を解くことによりDx ,
Dy ,Dz を求めることができる。
各ジャイロのドリフトを極めて厳密に求めることはでき
ないが、それでも、ジャイロセンサのみによって姿勢検
出を行なうのでは相対値しか測定できないことに起因し
て積み重ねていく程に誤差が集積していってしまうとこ
ろ、本発明によれば相対値でなく絶対値を測定できる磁
気センサによる値を参照するようにしているため、この
ような誤差の集積が生じてしまう虞れが回避される。
ロック図である。図3において、既述の図1と同様の機
能部については同一の符号を付して示し、それらの詳細
な説明は省略する。この実施の形態では、磁気ノイズの
多い環境下にも適用可能にするために、磁気センサから
の信号をフィルタリングすべく、磁気センサ系検出手段
20におけるA/D変換器27,28,29の各次段に
フィルタ回路271,281,及び291が夫々設けら
れている。しかし、フィルタリングするとこれによる時
間遅れが生じてしまうため、ジャイロセンサによる検出
信号とのタイミングにずれを生じることになる。図3の
実施の形態では、この時間遅れを補償するための構成を
備えている。
回路30内の各入力段に、フィルタ回路271,28
1,及び291による遅延時間に相応する遅延特性を得
るための遅延要素(回路)171,181,及び191
が夫々設けられている。尚、これらの遅延要素(回路)
171,181,及び191はジャイロ回転量・回転軸
演算回路30内に設けるのではなく、その入力側にそれ
ぞれ独立した回路として設けてもよいことは勿論であ
る。
中に遅延要素が介挿されると、系全体としての応答性が
犠牲になってしまうといった問題がある。本実施の形態
では、この問題に対処すべく、次のような構成をとって
いる。即ち、ジャイロセンサによる検出信号(A/D変
換後の、xyz各軸の回りの回転量のデータ)を遅延要
素171,181,及び191を介することなく、逐
次、データ格納用メモリ110にストアする。他方、ド
リフト推定演算回路50からは、フィルタ回路271,
281,及び291により遅延した磁気センサ系の検出
データ及び遅延要素171,181,及び191による
遅延を被ったジャイロセンサ系の検出データに依拠し
て、既述のようにジャイロセンサ系の出力に関するドリ
フト分のデータ(推定値)が算出される。
回路271,281,及び291による遅延時間に相応
する遅延を持った磁気センサの出力に対して上述のよう
にして算出されたドリフト分のデータを考慮し上記のド
リフトによる誤差が補正された回転量のデータ(推定
値)を適用して、回転量・回転軸を算出する。このよう
にして算出された回転量・回転軸が角度データ変換演算
回路70に供給され、ここで、ピッチ、ロール、ヨーの
各軸に関する回転角のデータとして当該被検出物体の姿
勢が算出される構成は既述の図1の実施の形態と同様で
ある。
ル、ヨーの各軸に関する回転角のデータは上述の遅延時
間の影響を被っているため、これら各データが得られる
時点を現在時点Tとすると、該各回転角のデータによっ
て表わされる事象の現実の発生時点は、現在時点Tより
も時間Δtだけ遡及した時点である。即ち、角度データ
変換演算回路70の出力は、時間Δtだけ以前の事象
(状態)を表わしている。そこで、上述のように遅延要
素による遅延を受けることなくデータ格納用メモリ11
0にストアされているデータを利用して現在時点Tにお
けるxyz各軸の回りの回転量のデータを推定する。
して説明する。尚、xyz各軸に係る各磁気センサ2
3,24,25の出力(フィルタ回路271,281,
及び291による遅延を受けた出力)は同時に得られる
ものとし、同様にxyz各軸に係る各ジャイロセンサ1
1,12,13の出力も同時に得られるものとしてある
ため、この図4では上記各磁気センサ23,24,25
の出力を総じて「磁気センサ系からのデータ」、同様に
各ジャイロセンサ11,12,13の出力を総じて「ジ
ャイロセンサ系からのデータ」と表記してある。
71,281,及び291による遅延時間Δtがあるた
め、図4に示されたように磁気センサ系から現在時点T
においてデータG4が得られるとき、この磁気センサ系
からのデータG4と同時刻における現象を表わすジャイ
ロセンサ系からのデータは、時点Tより時間Δtだけ過
去に遡及した時点で得られたデータJ1である。このた
め、当該現象(被検出物体の姿勢)が生起した時刻に関
する整合性が計られた両データである上記磁気センサ系
からのデータG4とジャイロセンサ系からのデータJ1
とを用いて、図3について上述したように、現在時点T
よりも時間Δtだけ遡及した時点におけるピッチ、ロー
ル、ヨーの各軸に関する回転角のデータとして当該被検
出物体の姿勢が算出される。
リには時点Tより過去のジャイロセンサ出力に依るデー
タがストアされているので、過去の時間Δtの期間に亘
るジャイロセンサ出力に依るデータ(ジャイロセンサ系
からのデータJ1,J2,J3)を用いてこの時間Δt
の間にピッチ、ロール、ヨーの各軸に関する回転角が如
何ほど変化したかをジャイロ回転角演算回路120で算
出する。このようにしてジャイロ回転角演算回路120
で算出されたピッチ、ロール、ヨーの各軸に関する回転
角のΔt間での変化量のデータを用いて、時間軸シフト
演算回路90において、角度データ変換演算回路70に
て先に得られた回転角のデータに対して補正をかけるこ
とにより、上述の遅延時間の影響が十分に補償されたデ
ータが得られる。
角演算回路120で算出されるデータはジャイロ系の出
力のみに依拠したものであるため、理論上はジャイロの
ドリフトが含まれることになるが、現実には、上述の時
間Δtは高々1秒以下の値であるため、実用上は何等支
障がなく無視できる程度のものである。本実施の形態に
よれば、上述のようにして、磁気センサ系に夫々フィル
タ回路271,281,及び291を介挿させたことに
よる遅延時間の影響を最小限のものとすることによっ
て、磁気ノイズの比較的多い環境下においてもノイズの
影響が除去され且つ応答性にも優れた姿勢検出装置を実
現することができる。
ブロック図である。この実施の形態はワンチップマイク
ロコンピュータを用いて装置を構成した例である。この
図5においても、ジャイロセンサ系検出手段及び磁気セ
ンサ系検出手段等既述の図1及び図3と同様の機能部に
ついては同一の符号を付して示し、それらの詳細な説明
は省略する。A/D変換器の機能部はA/Dコンバータ
入力ポート310として、通信インタフェース回路の機
能部は通信インタフェースポート320として、このワ
ンチップマイクロコンピュータ300に備えられてい
る。尚、これら機能部をワンチップマイクロコンピュー
タに備えず、所謂外付けの構成を採ることもできる。
におけるジャイロセンサ系検出手段及び磁気センサ系検
出手段の後段の各回路ブロックに相当する機能はワンチ
ップマイクロコンピュータ300によって実現される。
チップマイクロコンピュータ300の動作を表わすフロ
ーチャートである。ステップS1でジャイロセンサ系検
出手段及び磁気センサ系検出手段の出力データを夫々A
/Dコンバータ入力ポート310を介して読み込む。次
にステップS2でジャイロ系検出手段の出力データに基
づいて回転軸と回転量を算出する。また、ステップS3
で磁気センサ系検出手段の出力データに基づいて回転軸
を算出する。次にステップS4でジャイロセンサ系検出
手段及び磁気センサ系検出手段の両出力データの比較に
基づいて、算出の手法については図1及び図3の実施の
形態におけると同様にして、ドリフト量を推定するため
の演算を実行する。
れたドリフト量の値を用いて図1及び図3の実施の形態
のドリフト補正演算回路60におけると同様にしてこの
ドリフト量に関する補正を実行する。そしてステップS
6では誤差が補正された回転軸・回転量の表現による検
出出力をピッチ軸、ロール軸及びヨー軸回りの回動角に
よる表現に変換する。
納しておいたジャイロセンサ系検出手段の出力データを
利用して図3の実施の形態の時間軸シフト演算回路90
におけると同様にして時間軸をシフトする。次のステッ
プS8で、通信インタフェースポート320を介して出
力データをシリアル又はパラレルの形態で適宜の映像信
号処理装置等の外部のシステム(例えば、ホスト側コン
ピュータ)に伝送して、システムとしての所定の制御や
信号処理動作等が行なわれ得る。以上のようなワンチッ
プマイクロコンピュータを用いて構成した実施の形態で
は、部品点数の少ない簡素な構成の3軸姿勢検出装置が
実現される。
を回転軸・回転量表現からピッチ軸、ロール軸及びヨー
軸回りの回動角による表現に変換して出力するように構
成しているが、回転軸・回転量表現のまま出力するよう
に構成してもよいし、或いはまた、他の姿勢表現に変換
して出力するように構成してもよい。
部座標のX軸の方向が地磁気ベクトルの方向と一致する
ようにした場合を前提としている。このため、磁気セン
サが地磁気ベクトルの方向を向いているときが基準とな
るので、実際には、地平面と平行でないときにピッチ及
びロール軸回りの回動角が0になる。アプリケーション
によっては、地平面がXY平面となるように座標変換を
行なう必要があるので、所要に応じてこの座標変換を行
なえばよい。
る検出誤差が少なく、且つ構成が簡単なこの種の3軸姿
勢検出装置を提供することができる。即ち、請求項1の
発明によれば、誤差の少ない3軸姿勢検出装置が実現さ
れ、且つ、構成の簡素化とコストの低減も図られる。ま
た、請求項2の発明によれば、誤差の少ない3軸姿勢検
出装置が実現され、且つ、構成の簡素化とコスト低減も
図られる。特にこの発明では、ジャイロセンサによるド
リフトの影響が確実に補正された出力が得られる。請求
項3の発明によれば、誤差の少ない3軸姿勢検出装置が
実現され、且つ、構成の簡素化とコスト低減も図られ
る。特にこの発明では、磁気センサ系での応答遅れによ
る遅延時間の影響を、時間軸の遡及的シフト効果を奏す
る補正をかけて最小限のものとすることによって、磁気
ノイズの比較的多い環境下において磁気センサ系にフィ
ルタ等を介挿させた場合等においても応答性に優れた姿
勢検出装置を実現することができる。更に、請求項4の
発明によれば、ジャイロセンサの出力を回転量とこれに
係る回転軸を表わすデータに変換し、磁気センサの出力
を回転軸を表わすデータに変換してから、検出誤差(ジ
ャイロセンサに関するドリフト)の推定を行うことがで
きるため、このための演算が簡素化される。
検出装置を示すブロック図である。
び、第1乃至第3の磁気センサの配置と、目的物体と一
緒に動く相互に直交する3軸(x,y,z:以下センサ
座標xyzという)、並びに、基準となる動かない直交
座標系である外部座標XYZとの関係を示す模式図であ
る。
出装置を示すブロック図である。
ミング図である。
である。
コンピュータの動作を説明するためのフローチャートで
ある。
回路 40 磁気センサ回転軸演算回路 50 ドリフト推定演算回路 60 ドリフト補正演算回路 70 角度データ変換回路 80 通信インタフェース回路 90 時間軸シフト演算回路 110 データ格納用メモリ 120 ジャイロ回転角演算回路 171,181,191 遅延要素(回路) 271,281,291 フィルタ回路 300 ワンチップマイクロコンピュ
ータ 310 A/Dコンバータ入力ポート 320 通信インタフェースポート
Claims (4)
- 【請求項1】目的物体の空間内での姿勢を検出するため
に、該目的物体に関して仮定された相互に交差する3軸
の各軸の回りの回動角について、該回動角の検出の基礎
となる角速度を一次的に検出しこの一次的検出値の経時
的積分値である二次的検出値としてこれら回動角を検出
すべく該3軸のうちの各該当する軸に対応して設けられ
た第1のジャイロセンサ、第2のジャイロセンサ、及び
第3のジャイロセンサと、 上記3軸の各軸の回りの回動角について、該回動角の検
出の基礎となる方位角を検出すべく該3軸のうちの各該
当する軸に対応して設けられた第1の磁気センサ、第2
の磁気センサ、及び第3の磁気センサと、 上記第1乃至第3の磁気センサによる検出値に対し上記
第1乃至第3のジャイロセンサによる検出値に依拠して
得たデータを所定の演算により適用して誤差が補正され
た3軸姿勢検出出力を得る演算手段と、を備えてなるこ
とを特徴とする3軸姿勢検出装置。 - 【請求項2】目的物体の空間内での姿勢を検出するため
に、該目的物体に関して仮定された相互に交差する3軸
の各軸の回りの回動角について、該回動角の検出の基礎
となる角速度を一次的に検出しこの一次的検出値の経時
的積分値である二次的検出値としてこれら回動角を検出
すべく該3軸のうちの各該当する軸に対応して設けられ
た第1のジャイロセンサ、第2のジャイロセンサ、及び
第3のジャイロセンサと、 上記3軸の各軸の回りの回動角について、該回動角の検
出の基礎となる方位角を検出すべく該3軸のうちの各該
当する軸に対応して設けられた第1の磁気センサ、第2
の磁気センサ、及び第3の磁気センサと、 上記第1乃至第3のジャイロセンサによる回動角の検出
値および上記第1乃至第3の磁気センサによる方位角の
検出値に基づいて、上記第1乃至第3のジャイロセンサ
の出力たる上記二次的検出値としての回動角の検出値に
含まれる上記一次的検出値の誤差分に関する経時的積分
値であるドリフト成分の値を推定するドリフト推定演算
手段と、 上記第1乃至第3の磁気センサによる検出値に対し上記
ドリフト推定演算手段により算出されたドリフト成分の
推定値を適用して誤差が補正された3軸姿勢検出出力を
得るドリフト補正演算手段と、を備えてなることを特徴
とする3軸姿勢検出装置。 - 【請求項3】目的物体の空間内での姿勢を検出するため
に、該目的物体に関して仮定された相互に交差する3軸
の各軸の回りの回動角について、該回動角の検出の基礎
となる角速度を一次的に検出しこの一次的検出値の経時
的積分値である二次的検出値としてこれら回動角を検出
すべく該3軸のうちの各該当する軸に対応して設けられ
た第1のジャイロセンサ、第2のジャイロセンサ、及び
第3のジャイロセンサを含んで構成されたジャイロセン
サ系検出手段と、 上記3軸の各軸の回りの回動角について、該回動角の検
出の基礎となる方位角を検出すべく該3軸のうちの各該
当する軸に対応して設けられた第1の磁気センサ、第2
の磁気センサ、及び第3の磁気センサを含んで構成され
た磁気センサ系検出手段と、 上記ジャイロセンサ系検出手段による回動角の検出値た
るジャイロセンサ系検出データおよび上記磁気センサ系
検出手段による方位角の検出値たる磁気センサ系検出デ
ータを、これらジャイロセンサ系検出データの表わす事
象と磁気センサ系検出データの表わす事象との両事象の
時間的同時性が実効的に計られるように適用して、上記
ジャイロセンサ系検出データたる上記二次的検出値とし
ての回動角の検出値に含まれる上記一次的検出値の誤差
に関する経時的積分値であるドリフト成分の値を推定す
るドリフト推定演算手段と、 上記磁気センサ系検出データに対し上記ドリフト推定演
算手段により算出されたドリフト成分の推定値を適用し
て誤差が補正された3軸姿勢検出出力を得るドリフト補
正演算手段と、 上記ジャイロセンサ系検出手段に対する磁気センサ系検
出手段の検出の応答遅れに相応する時間以上の時間区間
に亘って実効的に遅延が与えられていない状態での上記
ジャイロセンサ系検出データを蓄積可能なデータ格納用
メモリと、 上記データ格納用メモリに蓄積されたジャイロセンサ系
検出データに依拠して上記磁気センサ系検出手段の当該
応答遅れに相応する時間内での上記3軸姿勢検出出力の
変化量の予測値を算出する変化量予測演算手段と、 上記ドリフト補正演算手段の出力たる3軸姿勢検出出力
に上記変化量予測演算手段で算出された3軸姿勢検出出
力の変化量の予測値を加味して実効的に時間軸を遡及し
たと等価な3軸姿勢検出出力を得る時間軸シフト演算手
段と、備えてなることを特徴とする3軸姿勢検出装置。 - 【請求項4】上記第1のジャイロセンサ、第2のジャイ
ロセンサ、及び第3のジャイロセンサの上記一次的検出
値たる上記3軸の各軸の回りの角速度のデータに基づい
て上記二次的検出値たる回動角を回転量とこれに係る回
転軸を表わすデータとして出力するように構成されたジ
ャイロ回転量・回転角演算手段と、 上記第1の磁気センサ、第2の磁気センサ、及び第3の
磁気センサの出力に基づいて回転軸を表わすデータを出
力するように構成された磁気センサ回転軸変換手段と、
を備えたことを特徴とする請求項1,2または3に記載
の3軸姿勢検出装置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP6444198A JPH11248456A (ja) | 1998-02-27 | 1998-02-27 | 3軸姿勢検出装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP6444198A JPH11248456A (ja) | 1998-02-27 | 1998-02-27 | 3軸姿勢検出装置 |
Publications (1)
Publication Number | Publication Date |
---|---|
JPH11248456A true JPH11248456A (ja) | 1999-09-17 |
Family
ID=13258379
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP6444198A Pending JPH11248456A (ja) | 1998-02-27 | 1998-02-27 | 3軸姿勢検出装置 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JPH11248456A (ja) |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2006329758A (ja) * | 2005-05-25 | 2006-12-07 | Nec Tokin Corp | 磁気探査装置 |
KR100738152B1 (ko) * | 1999-06-30 | 2007-07-10 | 데카 프로덕츠 리미티드 파트너쉽 | 차량 제어에 사용되는 피치 상태 신호를 생성하기 위한 방법, 차량의 피치 상태를 결정하기 위한 프로그램을 기록한 컴퓨터로 읽을 수 있는 매체, 차량의 피치 상태를 결정하기 위한 장치 및 상태 평가기 |
JP2007240194A (ja) * | 2006-03-06 | 2007-09-20 | Japan Radio Co Ltd | 方位計測装置 |
JP2009002934A (ja) * | 2007-04-25 | 2009-01-08 | Commiss Energ Atom | 実質的に不変な回転軸を検出するための方法および装置 |
JP2010145274A (ja) * | 2008-12-19 | 2010-07-01 | Panasonic Corp | 慣性センサ |
JP2018049000A (ja) * | 2016-07-29 | 2018-03-29 | イノベイティブ・ソリューションズ・アンド・サポート・インコーポレイテッド | 乗物の機首方位基準システムにおける軟鉄磁気擾乱を補償するための方法とシステム |
JP2018159569A (ja) * | 2017-03-22 | 2018-10-11 | カシオ計算機株式会社 | 姿勢算出装置、姿勢計測システム、及び姿勢算出方法 |
CN113866688A (zh) * | 2021-09-22 | 2021-12-31 | 西北工业大学 | 一种小姿态角条件下的三轴磁传感器误差校准方法 |
-
1998
- 1998-02-27 JP JP6444198A patent/JPH11248456A/ja active Pending
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR100738152B1 (ko) * | 1999-06-30 | 2007-07-10 | 데카 프로덕츠 리미티드 파트너쉽 | 차량 제어에 사용되는 피치 상태 신호를 생성하기 위한 방법, 차량의 피치 상태를 결정하기 위한 프로그램을 기록한 컴퓨터로 읽을 수 있는 매체, 차량의 피치 상태를 결정하기 위한 장치 및 상태 평가기 |
JP2006329758A (ja) * | 2005-05-25 | 2006-12-07 | Nec Tokin Corp | 磁気探査装置 |
JP2007240194A (ja) * | 2006-03-06 | 2007-09-20 | Japan Radio Co Ltd | 方位計測装置 |
JP2009002934A (ja) * | 2007-04-25 | 2009-01-08 | Commiss Energ Atom | 実質的に不変な回転軸を検出するための方法および装置 |
JP2010145274A (ja) * | 2008-12-19 | 2010-07-01 | Panasonic Corp | 慣性センサ |
JP2018049000A (ja) * | 2016-07-29 | 2018-03-29 | イノベイティブ・ソリューションズ・アンド・サポート・インコーポレイテッド | 乗物の機首方位基準システムにおける軟鉄磁気擾乱を補償するための方法とシステム |
JP2018159569A (ja) * | 2017-03-22 | 2018-10-11 | カシオ計算機株式会社 | 姿勢算出装置、姿勢計測システム、及び姿勢算出方法 |
CN113866688A (zh) * | 2021-09-22 | 2021-12-31 | 西北工业大学 | 一种小姿态角条件下的三轴磁传感器误差校准方法 |
CN113866688B (zh) * | 2021-09-22 | 2022-10-04 | 西北工业大学 | 一种小姿态角条件下的三轴磁传感器误差校准方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109163721B (zh) | 姿态测量方法及终端设备 | |
CN111678538B (zh) | 一种基于速度匹配的动态水平仪误差补偿方法 | |
Parsa et al. | Design and implementation of a mechatronic, all-accelerometer inertial measurement unit | |
US6711517B2 (en) | Hybrid inertial navigation method and device | |
CN109916395B (zh) | 一种姿态自主冗余组合导航算法 | |
Rios et al. | Fusion filter algorithm enhancements for a MEMS GPS/IMU | |
CN103512584A (zh) | 导航姿态信息输出方法、装置及捷联航姿参考*** | |
CN114485641B (zh) | 一种基于惯导卫导方位融合的姿态解算方法及装置 | |
CN109682377A (zh) | 一种基于动态步长梯度下降的姿态估计方法 | |
JP7025215B2 (ja) | 測位システム及び測位方法 | |
CN106370178B (zh) | 移动终端设备的姿态测量方法及装置 | |
JP2012173190A (ja) | 測位システム、測位方法 | |
CN113720330B (zh) | 一种亚角秒级的遥感卫星高精度姿态确定设计与实现方法 | |
CN107576977A (zh) | 基于多源信息自适应融合的无人机导航***及方法 | |
CN104121930B (zh) | 一种基于加表耦合的mems陀螺漂移误差的补偿方法 | |
JPH11248456A (ja) | 3軸姿勢検出装置 | |
CN113483765B (zh) | 一种卫星自主姿态确定方法 | |
CN112697143B (zh) | 高精度载体动态姿态测量方法及*** | |
CN111141285B (zh) | 一种航空重力测量装置 | |
CN110940357B (zh) | 一种用于旋转惯导单轴自对准的内杆臂标定方法 | |
CN110285811A (zh) | 卫星定位和惯性导航的融合定位方法和装置 | |
RU2502049C1 (ru) | Малогабаритная бесплатформенная инерциальная навигационная система средней точности, корректируемая от системы воздушных сигналов | |
CN112649001B (zh) | 一种小型无人机姿态与位置解算方法 | |
Parsa et al. | Design and mechatronic implementation of an accelerometer-based, kinematically redundant inertial measurement unit | |
He et al. | A novel drift compensating method for orientation measurement system in VR applications |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20041228 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20071119 |
|
RD03 | Notification of appointment of power of attorney |
Effective date: 20080122 Free format text: JAPANESE INTERMEDIATE CODE: A7423 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20080422 |
|
A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20080819 |