JPH0587585A - ストラツプダウン方式の姿勢検出装置 - Google Patents
ストラツプダウン方式の姿勢検出装置Info
- Publication number
- JPH0587585A JPH0587585A JP24760391A JP24760391A JPH0587585A JP H0587585 A JPH0587585 A JP H0587585A JP 24760391 A JP24760391 A JP 24760391A JP 24760391 A JP24760391 A JP 24760391A JP H0587585 A JPH0587585 A JP H0587585A
- Authority
- JP
- Japan
- Prior art keywords
- axis
- gyro
- signal
- azimuth
- acceleration
- 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.)
- Granted
Links
- 230000001133 acceleration Effects 0.000 claims abstract description 60
- 238000001514 detection method Methods 0.000 claims abstract description 14
- 230000005484 gravity Effects 0.000 claims description 5
- 230000005540 biological transmission Effects 0.000 abstract description 7
- 239000011159 matrix material Substances 0.000 description 19
- 238000010586 diagram Methods 0.000 description 9
- 230000008859 change Effects 0.000 description 8
- 230000009466 transformation Effects 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000000034 method Methods 0.000 description 3
- 230000009467 reduction Effects 0.000 description 3
- 239000013585 weight reducing agent Substances 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000005549 size reduction Methods 0.000 description 1
Landscapes
- Navigation (AREA)
Abstract
(57)【要約】
【目的】 精度を保持しつつ構成を簡略化することを目
的とする。 【構成】 右手直交３軸座標０−ＸＹＺと一致する取付
軸において、そのＸ軸には、Ｘ軸方向の加速度を検出す
るＸ加速度計１１ＡとＸ軸まわりの角速度を検出するＸ
ジャイロ１１Ｇとが取付けられ、そのＹ軸には、Ｙ軸方
向の加速度を検出するＹ加速度計１２ＡとＹ軸まわりの
角速度を検出するＹジャイロ１２Ｇとが取付られ、その
Ｚ軸には、Ｚ軸まわりの方位角を検出する磁気方位検出
器１３Ｙ′が取付けられたセンサブロック部１′と、こ
のセンサブロック部１′のＸ及びＹ加速度計１１Ａ及び
１２Ａから出力されるＸ及びＹ加速度信号、このＸ及び
Ｙジャイロ１１Ｇ及び１２Ｇから出力されるＸ及びＹジ
ャイロ信号、そしてこの磁気方位検出器１３Ｙ′から出
力される方位信号を入力とし、姿勢角、ロール角信号、
ピッチ角信号そして方位角信号を演算して、発信する演
算部２とより構成されたものである。
的とする。 【構成】 右手直交３軸座標０−ＸＹＺと一致する取付
軸において、そのＸ軸には、Ｘ軸方向の加速度を検出す
るＸ加速度計１１ＡとＸ軸まわりの角速度を検出するＸ
ジャイロ１１Ｇとが取付けられ、そのＹ軸には、Ｙ軸方
向の加速度を検出するＹ加速度計１２ＡとＹ軸まわりの
角速度を検出するＹジャイロ１２Ｇとが取付られ、その
Ｚ軸には、Ｚ軸まわりの方位角を検出する磁気方位検出
器１３Ｙ′が取付けられたセンサブロック部１′と、こ
のセンサブロック部１′のＸ及びＹ加速度計１１Ａ及び
１２Ａから出力されるＸ及びＹ加速度信号、このＸ及び
Ｙジャイロ１１Ｇ及び１２Ｇから出力されるＸ及びＹジ
ャイロ信号、そしてこの磁気方位検出器１３Ｙ′から出
力される方位信号を入力とし、姿勢角、ロール角信号、
ピッチ角信号そして方位角信号を演算して、発信する演
算部２とより構成されたものである。
Description
【０００１】
【産業上の利用分野】本発明は、ジャイロ、加速度計及
び磁気方位検出器を用いたストラップダウン方式による
姿勢検出装置に関する。
び磁気方位検出器を用いたストラップダウン方式による
姿勢検出装置に関する。
【０００２】
【従来の技術】従来のストラップダウン方式の姿勢検出
装置として図５及び図６に示す如きのものが提案されて
いる。図５は、従来のストラップダウン方式の姿勢検出
装置の一例の全体を示すブロック図で、このストラップ
ダウン方式の姿勢検出装置はセンサブロック部１と演算
部２より成る。図５のセンサブロック部１は、右手直交
３軸座標系と一致する取付軸１０（図５では、Ｚ軸方向
を下向垂直方向とし、図中の矢印は各センサの検出量の
（＋）の入力方向を示している。）において、Ｘ軸に
は、Ｘ軸方向の加速度を検出するＸ加速度計１１ＡとＸ
軸まわりの角速度を検出するＸジャイロ１１Ｇとが取付
けられ、Ｙ軸には、Ｙ軸方向の加速度を検出するＹ加速
度計１２ＡとＹ軸まわりの角速度を検出するＹジャイロ
１２Ｇとが取付けられ、Ｚ軸には、Ｚ軸方向の加速度を
検出するＺ加速度計１３ＡとＺ軸まわりの角速度を検出
するＺジャイロ１３Ｇと重力加速度方向の垂直軸まわり
の方位角を検出する磁気方位検出器１３Ｙとが取付けら
れている。このセンサブロック部１のこの各３軸の加速
度計１１Ａ、１２Ａ、１３Ａ、３軸のジャイロ１１Ｇ、
１２Ｇ、１３Ｇ及び磁気方位検出器１３Ｙからの出力信
号は、演算部２に入力する。その演算部２において、こ
の入力信号を用いて演算して、センサブロック部１の取
付軸１０のロール角信号ＲＯＬ、ピッチ角信号ＰＩＴ及
び方位角信号ＹＡＷを姿勢発信角として出力する。
装置として図５及び図６に示す如きのものが提案されて
いる。図５は、従来のストラップダウン方式の姿勢検出
装置の一例の全体を示すブロック図で、このストラップ
ダウン方式の姿勢検出装置はセンサブロック部１と演算
部２より成る。図５のセンサブロック部１は、右手直交
３軸座標系と一致する取付軸１０（図５では、Ｚ軸方向
を下向垂直方向とし、図中の矢印は各センサの検出量の
（＋）の入力方向を示している。）において、Ｘ軸に
は、Ｘ軸方向の加速度を検出するＸ加速度計１１ＡとＸ
軸まわりの角速度を検出するＸジャイロ１１Ｇとが取付
けられ、Ｙ軸には、Ｙ軸方向の加速度を検出するＹ加速
度計１２ＡとＹ軸まわりの角速度を検出するＹジャイロ
１２Ｇとが取付けられ、Ｚ軸には、Ｚ軸方向の加速度を
検出するＺ加速度計１３ＡとＺ軸まわりの角速度を検出
するＺジャイロ１３Ｇと重力加速度方向の垂直軸まわり
の方位角を検出する磁気方位検出器１３Ｙとが取付けら
れている。このセンサブロック部１のこの各３軸の加速
度計１１Ａ、１２Ａ、１３Ａ、３軸のジャイロ１１Ｇ、
１２Ｇ、１３Ｇ及び磁気方位検出器１３Ｙからの出力信
号は、演算部２に入力する。その演算部２において、こ
の入力信号を用いて演算して、センサブロック部１の取
付軸１０のロール角信号ＲＯＬ、ピッチ角信号ＰＩＴ及
び方位角信号ＹＡＷを姿勢発信角として出力する。
【０００３】図６は、図５の演算部２の一例の構成を示
すブロック図である。このセンサブロック部１の３軸の
ジャイロ１１Ｇ、１２Ｇ、１３Ｇの出力信号は、Ｘジャ
イロ信号ＧＸ、Ｙジャイロ信号ＧＹ、Ｚジャイロ信号Ｇ
Ｚ（以降、この３つのジャイロ信号をＧｉと記す場合も
ある）として演算部２のＣＴＭ演算部２０に入力され
る。このＣＴＭ演算部２０において、入力されたジャイ
ロ信号Ｇｉを用いてこの取付軸１０の座標系から、局地
座標系（Ｘ軸方向が北方向で、かつＺ軸方向が重力方向
に一致している座標系）に変換する３×３行列を演算す
る。
すブロック図である。このセンサブロック部１の３軸の
ジャイロ１１Ｇ、１２Ｇ、１３Ｇの出力信号は、Ｘジャ
イロ信号ＧＸ、Ｙジャイロ信号ＧＹ、Ｚジャイロ信号Ｇ
Ｚ（以降、この３つのジャイロ信号をＧｉと記す場合も
ある）として演算部２のＣＴＭ演算部２０に入力され
る。このＣＴＭ演算部２０において、入力されたジャイ
ロ信号Ｇｉを用いてこの取付軸１０の座標系から、局地
座標系（Ｘ軸方向が北方向で、かつＺ軸方向が重力方向
に一致している座標系）に変換する３×３行列を演算す
る。
【０００４】さらに、この３×３行列の要素により、局
地座標系に対する姿勢角をＸ軸に関してロール角信号Ｒ
ＯＬ、Ｙ軸に関してピッチ角信号ＰＩＴ及びＺ軸に関し
て方位角信号ＹＡＷを演算して、演算部２の出力とす
る。しかしながら、ＣＴＭ演算部２０で演算した３×３
行列には、演算する際に用いた３×３行列の初期値とセ
ンサブロック部１の取付軸１０の初期値との差等により
姿勢角の誤差が生じている。このＣＴＭ演算部２０に生
じた姿勢角の誤差の水平成分については水平演算部２１
と起立演算部２２とを用いて修正する。
地座標系に対する姿勢角をＸ軸に関してロール角信号Ｒ
ＯＬ、Ｙ軸に関してピッチ角信号ＰＩＴ及びＺ軸に関し
て方位角信号ＹＡＷを演算して、演算部２の出力とす
る。しかしながら、ＣＴＭ演算部２０で演算した３×３
行列には、演算する際に用いた３×３行列の初期値とセ
ンサブロック部１の取付軸１０の初期値との差等により
姿勢角の誤差が生じている。このＣＴＭ演算部２０に生
じた姿勢角の誤差の水平成分については水平演算部２１
と起立演算部２２とを用いて修正する。
【０００５】ＣＴＭ演算部２０から３×３の行列信号Ｃ
ＴＭＳと、このセンサブロック部１の３軸の加速度計１
１Ａ、１２Ａ、１３Ａの出力信号は、Ｘ加速度信号Ａ
Ｘ、Ｙ加速信号ＡＹ、Ｚ加速度信号ＡＺ（以降、３つの
加速度信号をＡｉと記す場合もある）として、演算部２
の水平演算部２１に入力される。この水平演算部２１に
おいて、入力された３つの加速度信号Ａｉと行列信号Ｃ
ＴＭＳとを用いて、この局地座標系に対する姿勢角誤差
のＸ軸方向姿勢角誤差ＥＲＲＸ及びＹ軸方向姿勢角誤差
ＥＲＲＹを演算し、出力する。この姿勢角誤差ＥＲＲ
Ｘ、ＥＲＲＹは、起立演算部２２に入力される。起立演
算部２２においてＣＴＭ演算部２０のＸ軸修正信号ＣＸ
とＹ軸修正信号ＣＹとを入力された姿勢角誤差ＥＲＲ
Ｘ、ＥＲＲＹを用いて演算し、出力する。
ＴＭＳと、このセンサブロック部１の３軸の加速度計１
１Ａ、１２Ａ、１３Ａの出力信号は、Ｘ加速度信号Ａ
Ｘ、Ｙ加速信号ＡＹ、Ｚ加速度信号ＡＺ（以降、３つの
加速度信号をＡｉと記す場合もある）として、演算部２
の水平演算部２１に入力される。この水平演算部２１に
おいて、入力された３つの加速度信号Ａｉと行列信号Ｃ
ＴＭＳとを用いて、この局地座標系に対する姿勢角誤差
のＸ軸方向姿勢角誤差ＥＲＲＸ及びＹ軸方向姿勢角誤差
ＥＲＲＹを演算し、出力する。この姿勢角誤差ＥＲＲ
Ｘ、ＥＲＲＹは、起立演算部２２に入力される。起立演
算部２２においてＣＴＭ演算部２０のＸ軸修正信号ＣＸ
とＹ軸修正信号ＣＹとを入力された姿勢角誤差ＥＲＲ
Ｘ、ＥＲＲＹを用いて演算し、出力する。
【０００６】また、このＣＴＭ演算部２０に生じた姿勢
角の誤差の垂直成分については、方位演算部２３を用い
て修正する。方位演算部２３において、このセンサブロ
ック部１のＺ軸に取付けられた方位検出器１３Ｙの出力
信号は、方位信号Ｈとして演算部２を介して入力される
一方、このＣＴＭ演算部２０で演算され、出力された方
位角信号ＹＡＷも入力され、入力された方位信号Ｈと方
位角信号ＹＡＷとを用いて、ＣＴＭ演算部２０のＺ軸修
正信号ＣＺを演算し、出力する。このＸ軸修正信号Ｃ
Ｘ、Ｙ軸修正信号ＣＹ及びＺ軸修正信号ＣＺは、ＣＴＭ
演算部２０に入力されて、姿勢角の誤差を修正するルー
プが構成される。
角の誤差の垂直成分については、方位演算部２３を用い
て修正する。方位演算部２３において、このセンサブロ
ック部１のＺ軸に取付けられた方位検出器１３Ｙの出力
信号は、方位信号Ｈとして演算部２を介して入力される
一方、このＣＴＭ演算部２０で演算され、出力された方
位角信号ＹＡＷも入力され、入力された方位信号Ｈと方
位角信号ＹＡＷとを用いて、ＣＴＭ演算部２０のＺ軸修
正信号ＣＺを演算し、出力する。このＸ軸修正信号Ｃ
Ｘ、Ｙ軸修正信号ＣＹ及びＺ軸修正信号ＣＺは、ＣＴＭ
演算部２０に入力されて、姿勢角の誤差を修正するルー
プが構成される。
【０００７】さらに具体的にセンサブロック部１の取付
軸１０の初期値と演算部２のＣＴＭ演算部２０の初期値
に差がある場合、最終的に姿勢角発信に含まれる誤差が
なくなる理由について述べる。このときの取付軸１０
は、局地座標系に対して、Ｚ軸まわりにγO 回転し、次
にＹ軸まわりにβO 回転し、最後にＸ軸まわりにαO 回
転していて、ＣＴＭ演算部２０の初期値は、局地座標系
と一致しているとする。このときの加速度信号Ａｉは近
似的に次式で表される。
軸１０の初期値と演算部２のＣＴＭ演算部２０の初期値
に差がある場合、最終的に姿勢角発信に含まれる誤差が
なくなる理由について述べる。このときの取付軸１０
は、局地座標系に対して、Ｚ軸まわりにγO 回転し、次
にＹ軸まわりにβO 回転し、最後にＸ軸まわりにαO 回
転していて、ＣＴＭ演算部２０の初期値は、局地座標系
と一致しているとする。このときの加速度信号Ａｉは近
似的に次式で表される。
【０００８】
【数１】
【０００９】ここで、ｇは重力加速度、ＴO は３×３行
列であり、数２で示す如くである。
列であり、数２で示す如くである。
【００１０】
【数２】
【００１１】ここで、αO 、βO 、γO は、取付軸１０
の局地座標系に対する微小角である。また、ＣＴＭ演算
部２０の行列信号ＣＴＭＳの初期値は、次式である。
の局地座標系に対する微小角である。また、ＣＴＭ演算
部２０の行列信号ＣＴＭＳの初期値は、次式である。
【００１２】
【数３】
【００１３】水平演算部２１において、局地座標系の
Ｘ、Ｙ軸方向の誤差角を演算するために、入力された行
列信号ＣＴＭＳと加速度信号Ａｉとを次式のように掛算
する。
Ｘ、Ｙ軸方向の誤差角を演算するために、入力された行
列信号ＣＴＭＳと加速度信号Ａｉとを次式のように掛算
する。
【００１４】
【数４】
【００１５】ここで、ＥＲＲｉはＸ、Ｙ．Ｚ軸の誤差角
である。この数４の内、Ｘ軸の誤差角は、−βO ｇとし
て、Ｙ軸の誤差角はαO ｇとして検出される。水平演算
部２１で検出した水平成分の誤差角ＥＲＲＸ、ＥＲＲＹ
は、起立演算部２２に入力される。起立演算部２２にお
いて、入力された誤差角ＥＲＲＸ、ＥＲＲＹを用いて、
次式の演算を行った後、Ｘ軸修正信号ＣＸとＹ軸修正信
号ＣＹとして出力される。
である。この数４の内、Ｘ軸の誤差角は、−βO ｇとし
て、Ｙ軸の誤差角はαO ｇとして検出される。水平演算
部２１で検出した水平成分の誤差角ＥＲＲＸ、ＥＲＲＹ
は、起立演算部２２に入力される。起立演算部２２にお
いて、入力された誤差角ＥＲＲＸ、ＥＲＲＹを用いて、
次式の演算を行った後、Ｘ軸修正信号ＣＸとＹ軸修正信
号ＣＹとして出力される。
【００１６】
【数５】
【００１７】
【数６】
【００１８】ここで、μは、比例ゲインである。取付軸
１０のＺ軸に取り付けられている磁気方位検出器１３Ｙ
から出力された方位信号Ｈと、ＣＴＭ演算部２０より発
信した方位角信号ＹＡＷとが、方位演算部２３に入力さ
れる。所で、取付軸１０のＺ軸に取付られている磁気方
位検出器１３Ｙは、重力加速度方向の垂直軸まわりの方
位を検出するので、この方位は、局地座標系に対する方
位角信号ＹＡＷと同じ座標系になっている。
１０のＺ軸に取り付けられている磁気方位検出器１３Ｙ
から出力された方位信号Ｈと、ＣＴＭ演算部２０より発
信した方位角信号ＹＡＷとが、方位演算部２３に入力さ
れる。所で、取付軸１０のＺ軸に取付られている磁気方
位検出器１３Ｙは、重力加速度方向の垂直軸まわりの方
位を検出するので、この方位は、局地座標系に対する方
位角信号ＹＡＷと同じ座標系になっている。
【００１９】方位演算部２３において、方位信号Ｈと方
位角信号ＹＡＷとは次式の様に演算されて、Ｚ軸修正信
号ＣＺを求める。
位角信号ＹＡＷとは次式の様に演算されて、Ｚ軸修正信
号ＣＺを求める。
【００２０】
【数７】
【００２１】
【数８】
【００２２】
【数９】 ここで、τは、方位時定数である。
【００２３】Ｘ軸修正信号ＣＸは、数５より、Ｙ軸修正
信号ＣＹは、数６より及びＺ軸修正信号は数９より得ら
れ、ＣＴＭ演算部２０に入力される。ＣＴＭ演算部２０
において、入力された３軸の修正信号ＣＸ、ＣＹ及びＣ
Ｚを用いて、修正行列ＣＴＭＣを作り、修正行列ＣＴＭ
ＣとＣＴＭ信号ＣＴＭＳとを用いて、次式の演算を行う
ことで、姿勢角の誤差を修正することができる。
信号ＣＹは、数６より及びＺ軸修正信号は数９より得ら
れ、ＣＴＭ演算部２０に入力される。ＣＴＭ演算部２０
において、入力された３軸の修正信号ＣＸ、ＣＹ及びＣ
Ｚを用いて、修正行列ＣＴＭＣを作り、修正行列ＣＴＭ
ＣとＣＴＭ信号ＣＴＭＳとを用いて、次式の演算を行う
ことで、姿勢角の誤差を修正することができる。
【００２４】
【数１０】
【００２５】ここで、ｔは、現在の時刻を表し、Δｔ
は、時間更新の間隔を表し、（ｔ＋Δｔ）は、現在の時
刻ｔから次の時刻（ｔ＋Δｔ）を表し、（ｔ，Δｔ）
は、時刻ｔ、時刻ｔ＋Δｔ間での変化量を表わす。
は、時間更新の間隔を表し、（ｔ＋Δｔ）は、現在の時
刻ｔから次の時刻（ｔ＋Δｔ）を表し、（ｔ，Δｔ）
は、時刻ｔ、時刻ｔ＋Δｔ間での変化量を表わす。
【００２６】
【数１１】
【００２７】数１１は離散型の行列で記述されているの
で、微分方程式の形に新ためて書き直すと、姿勢角の誤
差は次式のようになる。
で、微分方程式の形に新ためて書き直すと、姿勢角の誤
差は次式のようになる。
【００２８】
【数１２】
【００２９】よって、数１２を解けば、
【００３０】
【数１３】
【００３１】ここで、添字Ｏは、初期値を表わす。従っ
て数１１の修正をＣＴＭ演算部２０で行ったとき、行列
信号ＣＴＭＳに含まれた姿勢角誤差は、数１３で表さ
れ、その定常値は零となり、ＣＴＭ演算部２０からの姿
勢角（ロール角ＲＯＬ、ピッチ角ＰＩＴ及び方位角ＹＡ
Ｗ）に含まれる取付軸の初期値とＣＴＭ演算部２０の初
期値との差による誤差による成分は最終的にはなくな
り、その姿勢角は、取付軸１０の局地座標系に対する値
となる。次に、ＣＴＭ演算部２０において、入力された
３軸のジャイロ信号Ｇｉを用いて、取付軸１０の座標系
から局地座標系に変換する３×３の座標変換行列の演算
する方法について説明する。３軸のジャイロ信号Ｇｉは
時間（ｔ，ｔ＋Δｔ）で角速度を積分した角度である。
式で表せば、
て数１１の修正をＣＴＭ演算部２０で行ったとき、行列
信号ＣＴＭＳに含まれた姿勢角誤差は、数１３で表さ
れ、その定常値は零となり、ＣＴＭ演算部２０からの姿
勢角（ロール角ＲＯＬ、ピッチ角ＰＩＴ及び方位角ＹＡ
Ｗ）に含まれる取付軸の初期値とＣＴＭ演算部２０の初
期値との差による誤差による成分は最終的にはなくな
り、その姿勢角は、取付軸１０の局地座標系に対する値
となる。次に、ＣＴＭ演算部２０において、入力された
３軸のジャイロ信号Ｇｉを用いて、取付軸１０の座標系
から局地座標系に変換する３×３の座標変換行列の演算
する方法について説明する。３軸のジャイロ信号Ｇｉは
時間（ｔ，ｔ＋Δｔ）で角速度を積分した角度である。
式で表せば、
【００３２】
【数１４】
【００３３】ここで、ωｉは、Ｘ軸、Ｙ軸及びＺ軸ジャ
イロが検出した角速度である。３軸のジャイロ信号Ｇｉ
は、間隔Δｔでの変化量であり、この変化量を間隔Δｔ
毎に次式のように累積することにより、座標変換行列を
求める。すなわち、
イロが検出した角速度である。３軸のジャイロ信号Ｇｉ
は、間隔Δｔでの変化量であり、この変化量を間隔Δｔ
毎に次式のように累積することにより、座標変換行列を
求める。すなわち、
【００３４】
【数１５】
【００３５】ここで、ＣＴＭＳは、３×３の座標変換行
列Γ（ｔ，Δｔ）は、時間〔ｔ，ｔ＋Δｔ〕で変化した
角度行列で、
列Γ（ｔ，Δｔ）は、時間〔ｔ，ｔ＋Δｔ〕で変化した
角度行列で、
【００３６】
【数１６】
【００３７】ｔ＝０のとき、ＣＴＭＳ（０）は初期値で
あり、数３より
あり、数３より
【００３８】
【数１７】 となる。
【００３９】実際は、数１５のΓ（ｔ，Δｔ）と前述し
た数１０のＣＴＭ（ｔ，Δｔ）との演算が、ＣＴＭ演算
部２０において実行される。新ためて、その演算を記述
すると、次式となる。
た数１０のＣＴＭ（ｔ，Δｔ）との演算が、ＣＴＭ演算
部２０において実行される。新ためて、その演算を記述
すると、次式となる。
【００４０】
【数１８】
【００４１】また、ＣＴＭ演算部２０から発信するロー
ル角信号ＲＯＬ、ピッチ角信号ＰＩＴ及び方位角信号Ｙ
ＡＷの演算する方法は次のとおりである。最初、座標変
換行列の座標軸を局地座標軸と一致させておき、Ｚ軸ま
わりに方位角φ回転させ、次にＹ軸まわりにピッチ角θ
回転させ、最後にロール角φ回転させたときの３×３の
座標変換行列ＣＴＭＳは次式となる。
ル角信号ＲＯＬ、ピッチ角信号ＰＩＴ及び方位角信号Ｙ
ＡＷの演算する方法は次のとおりである。最初、座標変
換行列の座標軸を局地座標軸と一致させておき、Ｚ軸ま
わりに方位角φ回転させ、次にＹ軸まわりにピッチ角θ
回転させ、最後にロール角φ回転させたときの３×３の
座標変換行列ＣＴＭＳは次式となる。
【００４２】
【数１９】
【００４３】従って、数１９より姿勢角発信信号は、そ
れぞれ次式となる。
れぞれ次式となる。
【００４４】
【数２０】
【００４５】
【発明が解決しようとする課題】しかしながら、上述の
従来の装置は、３つのジャイロと３つの加速度計を用い
て、ストラップダウン方式を構成しているために、セン
サブロック部１の取付機構部が複雑になり、重量の軽減
化、小型化に際して制約が伴う不都合があった。さらに
重大なことは、ジャイロと加速度計の価格が装置自体の
価格の大部分を支えるために、従来の装置では価格の低
減化を容易に計れない不都合があった。また、使用する
ジャイロは、温度変化、振動等に対してドリフト変化が
極めて少ないものでなければ、そのドリフト変化により
姿勢角発信に誤差が含まれてしまう不都合があった。本
発明は斯る点に鑑み上述不都合を改善した新規なストラ
ップダウン方式の姿勢検出装置を提供せんとするもので
ある。
従来の装置は、３つのジャイロと３つの加速度計を用い
て、ストラップダウン方式を構成しているために、セン
サブロック部１の取付機構部が複雑になり、重量の軽減
化、小型化に際して制約が伴う不都合があった。さらに
重大なことは、ジャイロと加速度計の価格が装置自体の
価格の大部分を支えるために、従来の装置では価格の低
減化を容易に計れない不都合があった。また、使用する
ジャイロは、温度変化、振動等に対してドリフト変化が
極めて少ないものでなければ、そのドリフト変化により
姿勢角発信に誤差が含まれてしまう不都合があった。本
発明は斯る点に鑑み上述不都合を改善した新規なストラ
ップダウン方式の姿勢検出装置を提供せんとするもので
ある。
【００４６】
【課題を解決するための手段】本発明ストラップダウン
方式の姿勢検出装置は例えば図１、図２に示す如く、右
手直交３軸座標０−ＸＹＺと一致する取付軸において、
そのＸ軸には、Ｘ軸方向の加速度を検出するＸ加速度計
１１ＡとＸ軸のまわりの角速度を検出するＸジャイロ１
１Ｇとが取付けられ、そのＹ軸には、Ｙ軸方向の加速度
を検出するＹ加速度計１２ＡとＹ軸まわりの角速度を検
出するＹジャイロ１２Ｇとが取付けられ、そのＺ軸に
は、Ｚ軸まわりの方位角を検出する磁気方位検出器１３
Ｙ′が取付けられたセンサブロック部１′と、このセン
サブロック部１′のＸ及びＹ加速度計１１Ａ及び１２Ａ
から出力されるＸ及びＹ加速度信号、このＸ及びＹジャ
イロ１１Ｇ及び１２Ｇから出力されるＸ及びＹジャイロ
信号、そしてこの磁気方位検出器１３Ｙ′から出力され
る方位信号を入力し、姿勢角、ロール角信号、ピッチ角
信号そして方位角信号を演算して、発信する演算部２′
とより構成されたものである。
方式の姿勢検出装置は例えば図１、図２に示す如く、右
手直交３軸座標０−ＸＹＺと一致する取付軸において、
そのＸ軸には、Ｘ軸方向の加速度を検出するＸ加速度計
１１ＡとＸ軸のまわりの角速度を検出するＸジャイロ１
１Ｇとが取付けられ、そのＹ軸には、Ｙ軸方向の加速度
を検出するＹ加速度計１２ＡとＹ軸まわりの角速度を検
出するＹジャイロ１２Ｇとが取付けられ、そのＺ軸に
は、Ｚ軸まわりの方位角を検出する磁気方位検出器１３
Ｙ′が取付けられたセンサブロック部１′と、このセン
サブロック部１′のＸ及びＹ加速度計１１Ａ及び１２Ａ
から出力されるＸ及びＹ加速度信号、このＸ及びＹジャ
イロ１１Ｇ及び１２Ｇから出力されるＸ及びＹジャイロ
信号、そしてこの磁気方位検出器１３Ｙ′から出力され
る方位信号を入力し、姿勢角、ロール角信号、ピッチ角
信号そして方位角信号を演算して、発信する演算部２′
とより構成されたものである。
【００４７】また本発明ストラップダウン方式の姿勢検
出装置は、例えば図１、図２に示す如く上述において、
演算部２′に重力加速度方向の垂直軸まわりの方位角を
演算する機能、取付軸のＺ軸まわりの角速度を演算する
機能、このＺ軸方向の加速度を演算する機能、このＸ及
びＹジャイロ信号に含まれるＸ及びＹジャイロバイアス
成分を推定演算する機能及び、Ｘ及びＹジャイロバイア
ス成分を除去する機能を設け、Ｘ及びＹ加速度計１１Ａ
及び１２Ａ、Ｘ及びＹジャイロ１１Ｇ及び１２Ｇのおの
おの２個ずつを用いたストラップダウン方式の演算がで
きるようにしたものである。
出装置は、例えば図１、図２に示す如く上述において、
演算部２′に重力加速度方向の垂直軸まわりの方位角を
演算する機能、取付軸のＺ軸まわりの角速度を演算する
機能、このＺ軸方向の加速度を演算する機能、このＸ及
びＹジャイロ信号に含まれるＸ及びＹジャイロバイアス
成分を推定演算する機能及び、Ｘ及びＹジャイロバイア
ス成分を除去する機能を設け、Ｘ及びＹ加速度計１１Ａ
及び１２Ａ、Ｘ及びＹジャイロ１１Ｇ及び１２Ｇのおの
おの２個ずつを用いたストラップダウン方式の演算がで
きるようにしたものである。
【００４８】
【作用】本発明によればＸ及びＹ加速度計１１Ａ及び１
２Ａから出力されるＸ及びＹ加速度信号とＸ及びＹジャ
イロ１１Ｇ及び１２Ｇから出力されるＸ及びＹジャイロ
信号と磁気方位検出器１３Ｙ′から出力される方位信号
とより姿勢角、ロール角信号、ピッチ角信号及び方位角
信号を得るようにしているので、２個のジャイロ１１
Ｇ、１２Ｇ、２個の加速度計１１Ａ、１２Ａ及び磁気方
位検出器１３Ｙ′の構成で従来と同様の精度の姿勢検出
装置を得ることができる。
２Ａから出力されるＸ及びＹ加速度信号とＸ及びＹジャ
イロ１１Ｇ及び１２Ｇから出力されるＸ及びＹジャイロ
信号と磁気方位検出器１３Ｙ′から出力される方位信号
とより姿勢角、ロール角信号、ピッチ角信号及び方位角
信号を得るようにしているので、２個のジャイロ１１
Ｇ、１２Ｇ、２個の加速度計１１Ａ、１２Ａ及び磁気方
位検出器１３Ｙ′の構成で従来と同様の精度の姿勢検出
装置を得ることができる。
【００４９】また本発明によれば使用するジャイロのバ
イアス成分を推定し、除去する機能を有するので、振動
ジャイロ、ガスレートジャイロ等の比較的小型で、安価
なジャイロを使用できる。
イアス成分を推定し、除去する機能を有するので、振動
ジャイロ、ガスレートジャイロ等の比較的小型で、安価
なジャイロを使用できる。
【００５０】
【実施例】以下、図１及び図２を参照して本発明ストラ
ップダウン方式の姿勢検出装置の一実施例を説明する。
この図１及び図２において図５及び図６に対応する部分
には同一符号を付し、これらの説明は省略する。図１例
において、図５の従来例と異なる点は従来のセンサブロ
ック部１の取付軸１０のＺ軸に取付けられていたＺ加速
度計１３ＡとＺジャイロ１３Ｇが本例のセンサブロック
部１′の取付軸１０のＺ軸から取り除かれ、重力加速度
方向の垂直軸まわりの方位角を検出する磁気方位検出器
１３Ｙのかわりに、取付軸１０のＺ軸まわりの方位角を
検出する磁気方位検出器１３Ｙ′にかわったことと、図
５の従来例と異なる点は、演算部２′において、座標修
正部３０、Ｚジャイロ演算部３１、Ｚ加速度演算部４
０、ジャイロバイアス演算部５０及びジャイロバイアス
修正部５１が、新たに設けられていることである。
ップダウン方式の姿勢検出装置の一実施例を説明する。
この図１及び図２において図５及び図６に対応する部分
には同一符号を付し、これらの説明は省略する。図１例
において、図５の従来例と異なる点は従来のセンサブロ
ック部１の取付軸１０のＺ軸に取付けられていたＺ加速
度計１３ＡとＺジャイロ１３Ｇが本例のセンサブロック
部１′の取付軸１０のＺ軸から取り除かれ、重力加速度
方向の垂直軸まわりの方位角を検出する磁気方位検出器
１３Ｙのかわりに、取付軸１０のＺ軸まわりの方位角を
検出する磁気方位検出器１３Ｙ′にかわったことと、図
５の従来例と異なる点は、演算部２′において、座標修
正部３０、Ｚジャイロ演算部３１、Ｚ加速度演算部４
０、ジャイロバイアス演算部５０及びジャイロバイアス
修正部５１が、新たに設けられていることである。
【００５１】図１において、右手直交３軸座標系と一致
する取付軸１０に対してそのＸ軸には、Ｘ軸方向の加速
度を検出するＸ加速度計１１ＡとＸ軸まわりの角速度を
検出するＸジャイロ１１Ｇとが取付られ、そのＹ軸に
は、Ｙ軸方向の加速度を検出するＹ加速度計１２ＡとＹ
軸まわりの角速度を検出するＹジャイロとが取付られ、
そしてＺ軸には、Ｚ軸まわりの方位角を検出する磁気方
位検出器１３Ｙ′が取付けられたセンサブロック部１′
から演算部２′にこのＸ加速度計１１ＡからのＸ加速度
信号ＡＸ、Ｙ加速度計１２ＡからＹ加速度信号ＡＹ、Ｘ
ジャイロ１１ＧからのＸジャイロ信号ＧＸ、Ｙジャイロ
１２ＧからのＹジャイロ信号ＧＹそして磁気方位検出器
１３Ｙ′からの方位信号Ｈ′が供給される。この演算部
２′に供給されたＸ加速度信号ＡＸ、Ｙ加速度信号Ａ
Ｙ、Ｘジャイロ信号ＧＸ、Ｙジャイロ信号ＧＹそして方
位信号Ｈ′を用いて、演算し、センサブロック部１′の
取付軸１０の姿勢角、ロール角信号ＲＯＬ、ピッチ角信
号ＰＩＴ及び方位角信号ＹＡＷを姿勢発信角として出力
する。本例のより詳細な動作について図２、図３及び図
４を用いて説明する。
する取付軸１０に対してそのＸ軸には、Ｘ軸方向の加速
度を検出するＸ加速度計１１ＡとＸ軸まわりの角速度を
検出するＸジャイロ１１Ｇとが取付られ、そのＹ軸に
は、Ｙ軸方向の加速度を検出するＹ加速度計１２ＡとＹ
軸まわりの角速度を検出するＹジャイロとが取付られ、
そしてＺ軸には、Ｚ軸まわりの方位角を検出する磁気方
位検出器１３Ｙ′が取付けられたセンサブロック部１′
から演算部２′にこのＸ加速度計１１ＡからのＸ加速度
信号ＡＸ、Ｙ加速度計１２ＡからＹ加速度信号ＡＹ、Ｘ
ジャイロ１１ＧからのＸジャイロ信号ＧＸ、Ｙジャイロ
１２ＧからのＹジャイロ信号ＧＹそして磁気方位検出器
１３Ｙ′からの方位信号Ｈ′が供給される。この演算部
２′に供給されたＸ加速度信号ＡＸ、Ｙ加速度信号Ａ
Ｙ、Ｘジャイロ信号ＧＸ、Ｙジャイロ信号ＧＹそして方
位信号Ｈ′を用いて、演算し、センサブロック部１′の
取付軸１０の姿勢角、ロール角信号ＲＯＬ、ピッチ角信
号ＰＩＴ及び方位角信号ＹＡＷを姿勢発信角として出力
する。本例のより詳細な動作について図２、図３及び図
４を用いて説明する。
【００５２】図２は、本例の演算部２′のブロック図で
ある。この演算部２′において、ＣＴＭ演算部２０、水
平演算部２１及び起立演算部２２の動作は図６と同一な
ので省略し、新たに設けられた座標修正部３０、Ｚジャ
イロ演算部３１、Ｚ加速度演算部４０、ジャイロバイア
ス演算部５０及びジャイロバイアス修正部５１の動作に
ついて説明する。座標修正部３０において、入力された
方位角信号Ｈ′とＣＴＭ演算部から入力されたロール角
信号ＲＯＬ、ピッチ角信号ＰＩＴとを用いて、局地座標
系の方位角の修正方位信号ＨＣと、取付軸１０の座標系
の方位相当信号Ｈｚとを演算し、出力する。この座標修
正部３０の演算は、次式である。
ある。この演算部２′において、ＣＴＭ演算部２０、水
平演算部２１及び起立演算部２２の動作は図６と同一な
ので省略し、新たに設けられた座標修正部３０、Ｚジャ
イロ演算部３１、Ｚ加速度演算部４０、ジャイロバイア
ス演算部５０及びジャイロバイアス修正部５１の動作に
ついて説明する。座標修正部３０において、入力された
方位角信号Ｈ′とＣＴＭ演算部から入力されたロール角
信号ＲＯＬ、ピッチ角信号ＰＩＴとを用いて、局地座標
系の方位角の修正方位信号ＨＣと、取付軸１０の座標系
の方位相当信号Ｈｚとを演算し、出力する。この座標修
正部３０の演算は、次式である。
【００５３】
【数２１】
【００５４】
【数２２】
【００５５】ここで、ｆ1 、ｆ2 は、かっこ内の変数に
依存する関数である。この修正信号ＨＣは、方位演算部
２３に出力され、方位相当信号Ｈｚは、Ｚジャイロ演算
部３１に出力される。Ｚジャイロ演算部３１において、
入力された方位相当信号Ｈｚを用いて、等価Ｚジャイロ
信号Ｇｚ′を演算してＣＴＭ演算部２０に出力する。こ
の等価Ｚジャイロ信号Ｇｚ′は、１回前の方位相当信号
Ｈｚ（ｔ−Δｔ）と現在の方位相当信号Ｈｚ（ｔ）との
差分を求めることで得られる。よって、この等価Ｚジャ
イロ信号Ｇｚ′は、次式で示される。
依存する関数である。この修正信号ＨＣは、方位演算部
２３に出力され、方位相当信号Ｈｚは、Ｚジャイロ演算
部３１に出力される。Ｚジャイロ演算部３１において、
入力された方位相当信号Ｈｚを用いて、等価Ｚジャイロ
信号Ｇｚ′を演算してＣＴＭ演算部２０に出力する。こ
の等価Ｚジャイロ信号Ｇｚ′は、１回前の方位相当信号
Ｈｚ（ｔ−Δｔ）と現在の方位相当信号Ｈｚ（ｔ）との
差分を求めることで得られる。よって、この等価Ｚジャ
イロ信号Ｇｚ′は、次式で示される。
【００５６】
【数２３】
【００５７】ここで、Δｔは、演算の時間間隔である。
この等価Ｚ加速度信号Ａｚ′が数２３により得られたこ
とで、３軸のジャイロ信号を用いるＣＴＭ演算部２０の
演算動作が可能になる。Ｚ加速度演算部４０において、
入力されたＸ加速度信号ＡＸとＹ加速度信号ＡＹとを用
いて、等価Ｚ加速度信号Ａｚ′を演算して水平演算部２
１に出力する。まず、センサブロック部１′の取付軸１
０の座標系が、静止している場合、作用する加速度は重
力加速度だけであるから、図３を参照すると次式とな
る。
この等価Ｚ加速度信号Ａｚ′が数２３により得られたこ
とで、３軸のジャイロ信号を用いるＣＴＭ演算部２０の
演算動作が可能になる。Ｚ加速度演算部４０において、
入力されたＸ加速度信号ＡＸとＹ加速度信号ＡＹとを用
いて、等価Ｚ加速度信号Ａｚ′を演算して水平演算部２
１に出力する。まず、センサブロック部１′の取付軸１
０の座標系が、静止している場合、作用する加速度は重
力加速度だけであるから、図３を参照すると次式とな
る。
【００５８】
【数２４】
【００５９】ここで、ｇは重力加速度である。よって、
数２２より、等価Ｚ加速度信号Ａｚ′は、次式のとおり
得られる。
数２２より、等価Ｚ加速度信号Ａｚ′は、次式のとおり
得られる。
【００６０】
【数２５】
【００６１】又は、近似すれば、
【００６２】
【数２６】
【００６３】 このジャイロバイアス演算部５０とジャイロバイアス修
正部５１の動作を具体的に説明する。まず、図１に示さ
れる姿勢検出装置の誤差修正モデルは、次式のとおりで
ある。
正部５１の動作を具体的に説明する。まず、図１に示さ
れる姿勢検出装置の誤差修正モデルは、次式のとおりで
ある。
【００６４】
【数２７】
【００６５】
【００６６】
【数２８】
【００６７】
【００６８】
【数２９】
【００６９】
【００７０】
【数３０】
【００７１】
【００７２】
【数３１】
【００７３】
【００７４】
【数３２】
【００７５】である。
【００７６】
【数３３】
【００７７】のとき、数２４の右辺には、Ｘ、Ｙジャイ
ロバイアス信号ＢX、ＢY の影響がなくなる。
ロバイアス信号ＢX、ＢY の影響がなくなる。
【００７８】
【００７９】ジャイロバイアス演算部５０とジャイロバ
イアス修正部５１の動作を述べると以下となる。
イアス修正部５１の動作を述べると以下となる。
【００８０】
【数３４】
【００８１】また、センサブロック部１′での取付軸１
０の座標系が、比較的大きな加速度運動している場合、
作用する加速度は、重力加速度と運動加速度とになる。
従って、検出したＸ、Ｙ加速度信号ＡＸ、ＡＹを用い
て、Ｚ加速度演算部４０での数２５又は数２６の演算よ
り求められた等価Ｚ加速度信号Ａｚ′は、正しいＺ軸の
重力加速度の値にならない。そこで、取付軸１０のＸ、
Ｙ軸にそれぞれＸ速度計、Ｙ速度計を設け、このＸ、Ｙ
速度計からのＸ、Ｙ軸のそれぞれの速度信号ＶＸ、ＶＹ
を用いて、次式の演算を行うことで、運動加速度の影響
を受けない等価Ｚ加速度信号Ａｚ′が得られる。すなわ
ち、Ｚ加速度演算部４０において、数２５を修正して、
０の座標系が、比較的大きな加速度運動している場合、
作用する加速度は、重力加速度と運動加速度とになる。
従って、検出したＸ、Ｙ加速度信号ＡＸ、ＡＹを用い
て、Ｚ加速度演算部４０での数２５又は数２６の演算よ
り求められた等価Ｚ加速度信号Ａｚ′は、正しいＺ軸の
重力加速度の値にならない。そこで、取付軸１０のＸ、
Ｙ軸にそれぞれＸ速度計、Ｙ速度計を設け、このＸ、Ｙ
速度計からのＸ、Ｙ軸のそれぞれの速度信号ＶＸ、ＶＹ
を用いて、次式の演算を行うことで、運動加速度の影響
を受けない等価Ｚ加速度信号Ａｚ′が得られる。すなわ
ち、Ｚ加速度演算部４０において、数２５を修正して、
【００８２】
【数３５】
【００８３】とし、また数２６を修正して、
【００８４】
【数３６】
【００８５】とする。以上のことにより、取付軸１０
に、Ｘ、Ｙジャイロ１１Ｇ、１２Ｇ、Ｘ、Ｙ加速度計１
１Ａ、１２Ａ及び磁気方位検出器１３Ｙ′により、従来
と同じ姿勢検出装置が構成できる。尚本発明は上述実施
例に限ることなく本発明の要旨を逸脱することなく、そ
の他種々の構成が採り得ることは勿論である。
に、Ｘ、Ｙジャイロ１１Ｇ、１２Ｇ、Ｘ、Ｙ加速度計１
１Ａ、１２Ａ及び磁気方位検出器１３Ｙ′により、従来
と同じ姿勢検出装置が構成できる。尚本発明は上述実施
例に限ることなく本発明の要旨を逸脱することなく、そ
の他種々の構成が採り得ることは勿論である。
【００８６】
【発明の効果】本発明によれば、２個のジャイロ、２個
の加速度計そして方位センサの構成で従来の姿勢検出装
置と同様の精度を実現でき、それだけ構成か簡単となる
利益があると共に従来の姿勢検出装置では、その価格に
おいて３個のジャイロ、３個の加速度計そして方位セン
サの各価格の合計が大部分をしめていたので、本発明に
よる１個のジャイロと１個の加速度計の低減は、直接価
格の低減に連がる利益がある。
の加速度計そして方位センサの構成で従来の姿勢検出装
置と同様の精度を実現でき、それだけ構成か簡単となる
利益があると共に従来の姿勢検出装置では、その価格に
おいて３個のジャイロ、３個の加速度計そして方位セン
サの各価格の合計が大部分をしめていたので、本発明に
よる１個のジャイロと１個の加速度計の低減は、直接価
格の低減に連がる利益がある。
【００８７】また、本発明によれば１個のジャイロと１
個の加速度計の低減により、機構部の構成が簡単になる
ので、電子回路の低減等が期待でき、また部品数の低減
により柔軟な取付配置の構成が可能となり、重量の低減
化もまた容易となる利益がある。
個の加速度計の低減により、機構部の構成が簡単になる
ので、電子回路の低減等が期待でき、また部品数の低減
により柔軟な取付配置の構成が可能となり、重量の低減
化もまた容易となる利益がある。
【００８８】更にまた本発明によれば使用するジャイロ
バイアス成分を推定し、除去する機能を有するので、振
動ジャイロ、ガスレートジャイロ等の比較的小型で安価
なジャイロの使用が可能となるために、さらに低価格な
姿勢検出装置を提供できる利益があると共に本発明によ
れば使用する磁気方位検出器が、重力加速度方向の垂直
軸まわりの方位角を検出するタイプでも、取付軸１０の
Ｚ軸まわりの方位角を検出するタイプでも、わずかな修
正を行うことで対応できる利益があり、更に本発明の姿
勢検出装置が比較的大きな加速度運動をする場合、取付
軸１０のＸ、Ｙ軸に取付けたＸ、Ｙ速度計のＸ、Ｙ速度
を用いることで、容易に、加速度の影響を受けない姿勢
角発信を行うことができる利益がある。
バイアス成分を推定し、除去する機能を有するので、振
動ジャイロ、ガスレートジャイロ等の比較的小型で安価
なジャイロの使用が可能となるために、さらに低価格な
姿勢検出装置を提供できる利益があると共に本発明によ
れば使用する磁気方位検出器が、重力加速度方向の垂直
軸まわりの方位角を検出するタイプでも、取付軸１０の
Ｚ軸まわりの方位角を検出するタイプでも、わずかな修
正を行うことで対応できる利益があり、更に本発明の姿
勢検出装置が比較的大きな加速度運動をする場合、取付
軸１０のＸ、Ｙ軸に取付けたＸ、Ｙ速度計のＸ、Ｙ速度
を用いることで、容易に、加速度の影響を受けない姿勢
角発信を行うことができる利益がある。
【図１】本発明ストラップダウン方式の姿勢検出装置の
一実施例を示す構成図である。
一実施例を示す構成図である。
【図２】本発明の演算部２′の例を示す構成図である。
【図３】本発明の説明に供する線図である。
【図４】図３のジャイロバイアス演算部５０とジャイロ
バイアス修正部５１との説明に供する線図である。
バイアス修正部５１との説明に供する線図である。
【図５】従来の姿勢検出装置の例を示す構成図である。
【図６】従来の演算部２の例を示す構成図である。
１′ センサブロック部 ２′ 演算部 １１Ａ Ｘ加速度計 １１Ｇ Ｘジャイロ １２Ａ Ｙ加速度計 １２Ｇ Ｙジャイロ １３Ｙ′ 磁気方位検出器 ２０ ＣＴＭ演算部 ２１ 水平演算部 ２２ 起立演算部 ２３ 方位演算部 ３０ 座標修正部 ３１ Ｚジャイロ演算部 ４０ Ｚ加速度演算部 ５０ ジャイロバイアス演算部 ５１ ジャイロバイアス修正部
フロントページの続き (72)発明者 中村 慎一 東京都大田区南蒲田２丁目16番46号 株式 会社トキメツク内
Claims (2)
- 【請求項１】 右手直交３軸座標Ｏ−ＸＹＺと一致する
取付軸において、そのＸ軸には、Ｘ軸方向の加速度を検
出するＸ加速度計とＸ軸まわりの角速度を検出するＸジ
ャイロとが取付られ、そのＹ軸には、Ｙ軸方向の加速度
を検出するＹ加速度計とＹ軸まわりの角速度を検出する
Ｙジャイロとが取付られ、そのＺ軸には、Ｚ軸まわりの
方位角を検出する磁気方位検出器が取付けられたセンサ
ブロック部と、 該センサブロック部の上記Ｘ及びＹ加速度計から出力さ
れるＸ及びＹ加速度信号、上記Ｘ及びＹジャイロから出
力されるＸ及びＹジャイロ信号、そして上記磁気方位検
出器から出力される方位信号を入力とし、姿勢角、ロー
ル角信号、ピッチ角信号そして方位角信号を演算して、
発信する演算部とより構成されることを特徴とするスト
ラップダウン方式の姿勢検出装置。 - 【請求項２】 請求項１記載のストラップダウン方式の
姿勢検出装置において、上記演算部に、重力加速度方向
の垂直軸まわりの方位角を演算する機能、上記取付軸の
Ｚ軸まわりの角速度を演算する機能、上記Ｚ軸方向の加
速度を演算する機能、上記Ｘ及びＹジャイロ信号に含ま
れるＸ及びＹジャイロバイアス成分を推定演算する機能
及び、上記Ｘ及びＹジャイロバイアス成分を除去する機
能を設け、上記Ｘ及びＹ加速度計、上記Ｘ及びＹジャイ
ロのおのおの２個ずつを用いたストラップダウン方式の
演算ができるようにしたことを特徴とするストラップダ
ウン方式の姿勢検出装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP3247603A JP3038452B2 (ja) | 1991-09-26 | 1991-09-26 | ストラップダウン方式の姿勢検出装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP3247603A JP3038452B2 (ja) | 1991-09-26 | 1991-09-26 | ストラップダウン方式の姿勢検出装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH0587585A true JPH0587585A (ja) | 1993-04-06 |
| JP3038452B2 JP3038452B2 (ja) | 2000-05-08 |
Family
ID=17165968
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP3247603A Expired - Fee Related JP3038452B2 (ja) | 1991-09-26 | 1991-09-26 | ストラップダウン方式の姿勢検出装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3038452B2 (ja) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2011227017A (ja) * | 2010-04-23 | 2011-11-10 | Univ Of Tokyo | 慣性センサ，磁気センサおよび速度計を用いた移動体の姿勢推定装置および姿勢推定方法 |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP3837533B2 (ja) | 2003-01-15 | 2006-10-25 | 独立行政法人産業技術総合研究所 | 姿勢角処理装置および姿勢角処理方法 |
| JP4615287B2 (ja) | 2004-11-01 | 2011-01-19 | 東京計器株式会社 | 方位姿勢検出装置 |
| US9008996B2 (en) | 2008-07-02 | 2015-04-14 | National Institute Of Advanced Industrial Science And Technology | Moving body positioning device |
-
1991
- 1991-09-26 JP JP3247603A patent/JP3038452B2/ja not_active Expired - Fee Related
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2011227017A (ja) * | 2010-04-23 | 2011-11-10 | Univ Of Tokyo | 慣性センサ，磁気センサおよび速度計を用いた移動体の姿勢推定装置および姿勢推定方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| JP3038452B2 (ja) | 2000-05-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP5569681B2 (ja) | 慣性センサ，磁気センサおよび速度計を用いた移動体の姿勢推定装置および姿勢推定方法 | |
| JP4615287B2 (ja) | 方位姿勢検出装置 | |
| US3924824A (en) | Cross track strapdown inertial quidance system | |
| JPH09106322A (ja) | ヘッドマウントディスプレイにおける姿勢角検出装置 | |
| JP3763435B2 (ja) | 姿勢角検出装置 | |
| JP3960574B2 (ja) | 姿勢角検出装置 | |
| JP3038452B2 (ja) | ストラップダウン方式の姿勢検出装置 | |
| JP4023889B2 (ja) | 姿勢角検出装置 | |
| RU2107897C1 (ru) | Способ инерциальной навигации | |
| JP2913122B2 (ja) | ストラップダウン型ジャイロ装置 | |
| JP3992206B2 (ja) | 姿勢角検出装置 | |
| JP3425689B2 (ja) | 慣性装置 | |
| JPS62106385A (ja) | 乗物の運行角度を計算するためのドップラ慣性ループ | |
| EP1206683B1 (en) | Integrated inertial/vms navigation system | |
| JPH06201863A (ja) | ストラップダウン方式の姿勢検出装置 | |
| JP3960575B2 (ja) | 姿勢角検出装置 | |
| JP3360089B2 (ja) | 慣性装置 | |
| JP2000356520A (ja) | 姿勢角検出装置 | |
| Huddle | Advances in strapdown systems for geodetic applications | |
| JP2004132769A (ja) | 姿勢角検出装置 | |
| CN119687967B (zh) | 一种发射系的摇摆基座回溯修正对准方法 | |
| JP2001033253A (ja) | 姿勢角検出装置 | |
| JP3357903B2 (ja) | 慣性装置 | |
| JPH06109477A (ja) | ストラップダウン方式の姿勢検出装置 | |
| JP3884742B2 (ja) | 方位計 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| LAPS | Cancellation because of no payment of annual fees |