JPH06201863A - ストラップダウン方式の姿勢検出装置 - Google Patents

Abstract

(57)【要約】 （修正有） 【目的】 航行体の旋回運動時に於いて、加速度誤差を
生じないようにし、かつ横すべり角の成分を外部に出力
できるようにする。 【構成】 右手直交３軸座標系Ｏ−ＸＹＺと一致する取
付軸１０において、そのＸ軸には、Ｘ軸方向の加速度及
び速度をそれぞれ検出するＸ加速度計１１Ａ及びＸ速度
計１１ＶとＸ軸まわりの角速度を検出するＸジャイロ１
１Ｇとが取付けられ、Ｙ軸には、Ｙ軸方向の加速度を検
出するＹ加速度計１２ＡとＹ軸まわりの角速度を検出す
るＹジャイロ１２Ｇとが取り付けられ、Ｚ軸には、Ｚ加
速度計１３ＡとＺジャイロ１３Ｇと磁気方位検出器１３
Ｙとが取付けられたセンサブロック部１と、Ｘ，Ｙ，Ｚ
加速度計、Ｘ，Ｙ，Ｚジャイロ信号、及び方位信号を入
力し、姿勢角であるロール角信号、ピッチ角信号、方位
角信号及び横すべり角信号を演算して、出力する演算部
２′とにより構成されたものである。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、速度計、ジャイロ、加
速度計及び磁気方位検出器を用いて航行体の横すべり角
成分を出力できるようにしたストラップダウン方式によ
る姿勢検出装置に関する。

【０００２】

【従来の技術】従来のストラップダウン方式の姿勢検出
装置として図４及び図５に示す如きのものが提案されて
いる。図４は、従来のストラップダウン方式の姿勢検出
装置の一例の全体を示すブロック図で、このストラップ
ダウン方式の姿勢検出装置はセンサブロック部１と演算
部２より成る。

【０００３】このセンサブロック部１は、右手直交３軸
座標系と一致する取付軸１０（図４では、Ｚ軸方向を下
向垂直方向とし、図中の矢印は各センサの検出量の
（＋）の入力方向を示している。）において、Ｘ軸（こ
のＸ軸は、運動に関して、主方向とする）には、Ｘ軸方
向の加速度及び速度をそれぞれ検出するＸ加速度計１１
Ａ及びＸ速度計１１ＶとＸ軸まわりの角速度を検出する
Ｘジャイロ１１Ｇとが取付けられ、Ｙ軸には、Ｙ軸方向
の加速度を検出するＹ加速度計１２ＡとＹ軸まわりの角
速度を検出するＹジャイロ１２Ｇとが取付けられ、Ｚ軸
には、Ｚ軸方向の加速度を検出するＺ加速度計１３Ａと
Ｚ軸まわりの角速度を検出するＺジャイロ１３Ｇと重力
加速度方向の垂直軸まわりの方位角を検出する磁気方位
検出器１３Ｙとが取付けられている。このセンサブロッ
ク部１のこの各３軸の加速度計１１Ａ、１２Ａ、１３
Ａ、３軸のジャイロ１１Ｇ、１２Ｇ、１３Ｇ及び磁気方
位検出器１３Ｙ及び速度計１１Ｖからの出力信号は、演
算部２に入力する。

【０００４】その演算部２において、この入力信号を用
いて演算して、センサブロック部１の取付軸１０のロー
ル角信号ＲＯＬ、ピッチ角信号ＰＩＴ及び方位角信号Ｙ
ＡＷを姿勢角として出力する。

【０００５】図５は、図４の演算部２の一例の構成を示
すブロック図である。このセンサブロック部１の３軸の
ジャイロ１１Ｇ、１２Ｇ、１３Ｇの出力信号は、Ｘジャ
イロ信号ＧＸ、Ｙジャイロ信号ＧＹ、Ｚジャイロ信号Ｇ
Ｚ（以降、この３つのジャイロ信号をＧｉと記す場合も
ある）として演算部２の後述するジャイロバイアス修正
部２６を介してＣＴＭ演算部２０に入力される。このＣ
ＴＭ演算部２０において、入力されたジャイロ信号Ｇｉ
を用いてこの取付軸１０の座標系から、局地座標系（Ｘ
軸方向が北方向で、かつＺ軸方向が重力方向に一致して
いる座標系）に変換する３×３行列を演算する。

【０００６】さらに、この３×３行列の要素により、局
地座標系に対する姿勢角をＸ軸に関してロール角信号Ｒ
ＯＬ、Ｙ軸に関してピッチ角信号ＰＩＴ及びＺ軸に関し
て方位角信号ＹＡＷを演算して、演算部２の出力とす
る。しかしながら、ＣＴＭ演算部２０で演算した３×３
行列には、演算する際に用いた３×３行列の初期値とセ
ンサブロック部１の取付軸１０の初期値との差等により
姿勢角の誤差が生じている。このＣＴＭ演算部２０に生
じた姿勢角の誤差の水平成分については水平誤差演算部
２１と加速度誤差修正部２２と修正量演算部２４とを用
いて修正する。

【０００７】ＣＴＭ演算部２０から３×３の行列信号Ｃ
ＴＭＳと、このセンサブロック部１の３軸の加速度計１
１Ａ、１２Ａ、１３Ａの出力信号は、Ｘ加速度信号Ａ
Ｘ、Ｙ加速度信号ＡＹ、Ｚ加速度信号ＡＺ（以降、３つ
の加速度信号をＡｉと記す場合もある）として、演算部
２の水平誤差演算部２１に入力される。この水平誤差演
算部２１において、入力された３つの加速度信号Ａｉと
行列信号ＣＴＭＳとを用いて、この局地座標系に対する
姿勢角誤差のＸ軸方向姿勢角誤差ＥＲＲＸ′及びＹ軸方
向姿勢角誤差ＥＲＲＹ′を演算し、出力する。この姿勢
角誤差ＥＲＲＸ′、ＥＲＲＹ′は、加速度誤差修正部２
２に入力される。加速度誤差修正部２２において、姿勢
角誤差ＥＲＲＸ′，ＥＲＲＹ′に含まれる運動加速度成
分は、このセンサブロック部１のＸ軸に取付けられたＸ
速度計１１Ｖの出力信号であるＸ軸速度信号ＶＸとＣＴ
Ｍ演算部２０の方位角信号ＹＡＷとを用いて、局地座標
系の修正運動加速度成分を演算し、差し引くことで、除
去されてＸ，Ｙ軸姿勢角誤差ＥＲＲＸ，ＥＲＲＹを出力
する。Ｘ，Ｙ軸姿勢角誤差ＥＲＲＸ，ＥＲＲＹは、修正
量演算部２４に入力される。

【０００８】修正量演算部２４において、ＣＴＭ演算部
２０のＸ軸、Ｙ軸のそれぞれの座標修正量であるＸ軸修
正信号ＣＸとＹ軸修正信号ＣＹとを入力されたＸ，Ｙ軸
姿勢角誤差ＥＲＲＸ，ＥＲＲＹを用いて演算し、出力す
る。

【０００９】一方、このＣＴＭ演算部２０に生じた姿勢
角の誤差の垂直成分については、方位誤差演算部２３と
修正演算部２４とを用いて修正する。方位誤差演算部２
３において、入力された方位信号Ｈと共に、このＣＴＭ
演算部２０で演算され、出力された方位角信号ＹＡＷも
入力され、方位信号Ｈと方位角信号ＹＡＷとの偏差量を
演算して、方位角誤差ＥＲＲＺを出力する。

【００１０】方位角誤差ＥＲＲＺは、修正量演算部２４
に入力される。修正量演算部２４において、方位角誤差
ＥＲＲＺを用いて、ＣＴＭ演算部２０のＺ軸の座標修正
量であるＺ軸修正信号ＣＺを演算し、出力する。このＸ
軸修正信号ＣＸ、Ｙ軸修正信号ＣＹ及びＺ軸修正信号Ｃ
ＺはＣＴＭ演算部２０に入力されて、姿勢角の誤差を修
正するフィードバックループが構成される。

【００１１】また、３つのジャイロ信号Ｇｉに含まれる
バイアス成分によりＣＴＭ演算部２０に生じた姿勢角、
方位角の誤差については、ジャイロバイアス演算部２
５、ジャイロバイアス修正部２６を用いて修正する。

【００１２】

【００１３】

【００１４】さらに具体的にセンサブロック部１の取付
軸１０の初期値と演算部２のＣＴＭ演算部２０の初期値
に差がある場合、最終的に姿勢角発信に含まれる誤差が
なくなる理由について述べる。ただし、ここでは簡単の
ため、ジャイロバイアス成分Ｂｉはないものとする。ま
た、加速度誤差修正部２２の詳細な操作は、省略する。
このときの取付軸１０は、局地座標系に対して、Ｚ軸ま
わりにγ₀ 回転し、次にＹ軸まわりにβ₀ 回転し、最後
にＸ軸まわりにα₀ 回転していて、ＣＴＭ演算部２０の
初期値は、局地座標系と一致しているとする。このとき
の加速度信号Ａｉは近似的に次式で表される。

【００１５】

【数１】

【００１６】ここで、ｇは重力加速度Ｔ_O は、３×３行
列であり、数２で示す如くである。

【００１７】

【数２】

【００１８】ここで、α_O 、β_O 、γ_O は、取付軸１０
の局地座標系に対する微小角である。また、ＣＴＭ演算
部２０の行列信号ＣＴＭＳの初期値は、次式である。

【００１９】

【数３】

【００２０】水平誤差演算部２１と加速度誤差修正部２
２において、局地座標系のＸ、Ｙ軸方向の誤差角ＥＲＲ
Ｘ，ＥＲＲＹを演算するために、入力された行列信号Ｃ
ＴＭＳと加速度信号Ａｉとを次式のように掛算する。

【００２１】

【数４】 従って、Ｘ，Ｙ軸誤差角ＥＲＲＸ，ＥＲＲＹは、次式と
なる。

【００２２】

【数５】ＥＲＲＸ＝ｇ・α₀ ÷ｇ＝α₀

【数６】ＥＲＲＹ＝−β₀ ここで、誤差角は、角度単位であるので、重力加速度ｇ
で除算している。

【００２３】また、局地座標系のＺ軸まわりの誤差角Ｅ
ＲＲＺを演算するため、取付軸１０のＺ軸に取り付けら
れている磁気方位検出器１３Ｙから出力された方位信号
Ｈと、ＣＴＭ演算部２０より発信した方位角信号ＹＡＷ
とが、方位誤差演算部２３に入力される。

【００２４】所で、取付軸１０のＺ軸に取付られている
磁気方位検出器１３Ｙは、重力加速度方向の垂直軸まわ
りの方位を検出するので、この方位は、局地座標系に対
する方位角信号ＹＡＷと同じ座標系になっている。

【００２５】方位誤差演算部２３において、方位信号Ｈ
と方位角信号ＹＡＷとは次式の様に演算されて、方位角
誤差ＥＲＲＺを求める。

【００２６】

【数７】Ｈ＝γ₀

【００２７】

【数８】 ＹＡＷ＝０（ＣＴＭ演算部２０の方位角の初期値より） ＥＲＲＺ＝Ｈ−ＹＡＷ 演算された３軸誤差ＥＲＲｉは、修正量演算部２４に入
力される。

【００２８】修正量演算部２４において、入力された３
軸誤差ＥＲＲｉを用いて、次式の演算を行った後、Ｘ軸
修正信号ＣＸ，Ｙ軸修正信号ＣＹ，Ｚ軸修正信号ＣＺと
して出力される。

【００２９】

【数９】 ここで、τは、時定数の単位を持つ定数である。

【００３０】Ｘ軸修正信号ＣＸ、Ｙ軸修正信号ＣＹ及び
Ｚ軸修正信号ＣＺは修正量演算部２４より得られ、ＣＴ
Ｍ演算部２０に入力される。ＣＴＭ演算部２０におい
て、入力された３軸の修正信号ＣＸ、ＣＹ及びＣＺを用
いて、修正行列ＣＴＭＣを作り、修正行列ＣＴＭＣとＣ
ＴＭ信号ＣＴＭＳとを用いて、次式の演算を行うこと
で、姿勢角の誤差を修正することができる。

【００３１】

【数１０】 ＣＴＭＳ（ｔ＋Δｔ）＝ＣＴＭＳ（ｔ）・ＣＴＭＣ（ｔ，Δｔ） ここで、ｔは、現在の時刻を表し、Δｔは、時間更新の
間隔を表し、（ｔ＋Δｔ）は、現在の時刻ｔから次の時
刻（ｔ＋Δｔ）を表し、（ｔ，Δｔ）は、時刻ｔ、時刻
ｔ＋Δｔ間での変化量を表わす。

【００３２】

【数１１】

【００３３】数１１は離散型の行列で記述されているの
で、微分方程式の形に新ためて書き直すと、姿勢角の誤
差は次式のようになる。

【００３４】

【数１２】 よって、数１２を解けば、

【００３５】

【数１３】 となる。

【００３６】ここで、添字Ｏは、初期値を表わす。従っ
て数１１の修正をＣＴＭ演算部２０で行ったとき、行列
信号ＣＴＭＳに含まれた姿勢角誤差は、数１３で表さ
れ、その定常値は零となり、ＣＴＭ演算部２０からの姿
勢角（ロール角ＲＯＬ、ピッチ角ＰＩＴ及び方位角ＹＡ
Ｗ）に含まれる取付軸の初期値とＣＴＭ演算部２０の初
期値との差による誤差による成分は最終的にはなくな
り、その姿勢角は、取付軸１０の局地座標系に対する値
となる。次に、ＣＴＭ演算部２０において、入力された
３軸のジャイロ信号Ｇｉを用いて、取付軸１０の座標系
から局地座標系に変換する３×３の座標変換行列の演算
する方法について説明する。

【００３７】３軸のジャイロ信号Ｇｉは時間（ｔ，ｔ＋
Δｔ）で角速度を積分した角度である。式で表せば、

【数１４】

【００３８】ここで、ωｉは、Ｘ軸、Ｙ軸及びＺ軸ジャ
イロが検出した角速度である。３軸のジャイロ信号Ｇｉ
は、間隔Δｔでの変化量であり、この変化量を間隔Δｔ
毎に次式のように累積することにより、座標変換行列を
求める。すなわち、

【００３９】

【数１５】 ＣＴＭＳ（ｔ＋Δｔ）＝ＣＴＭＳ（ｔ）・Γ（ｔ，Δｔ）

【００４０】ここで、ＣＴＭＳは、３×３の座標変換行
列Γ（ｔ，Δｔ）は、時間〔ｔ，ｔ＋Δｔ〕で変化した
角度行列で、

【００４１】

【数１６】 である。

【００４２】ｔ＝０のとき、ＣＴＭＳ（０）は初期値で
あり、数３より

【００４３】

【数１７】ＣＴＭＳ（０）＝ＣＴＭＳ_O となる。

【００４４】実際は、数１５のΓ（ｔ，Δｔ）と前述し
た数１０のＣＴＭ（ｔ，Δｔ）との演算が、ＣＴＭ演算
部２０において実行される。新ためて、その演算を記述
すると、次式となる。

【００４５】

【数１８】

【００４６】また、ＣＴＭ演算部２０から発信するロー
ル角信号ＲＯＬ、ピッチ角信号ＰＩＴ及び方位角信号Ｙ
ＡＷの演算する方法は次のとおりである。最初、座標変
換行列の座標軸を局地座標軸と一致させておき、Ｚ軸ま
わりに方位角φ回転させ、次にＹ軸まわりにピッチ角θ
回転させ、最後にロール角φ回転させたときの３×３の
座標変換行列ＣＴＭＳは次式となる。

【００４７】

【数１９】

【００４８】従って、数１９より姿勢角出力信号は、そ
れぞれ次式となる。

【００４９】

【数２０】

【００５０】さらに、前記したジャイロバイアス演算部
２５とジャイロバイアス修正部２６の動作を具体的に説
明する。まず、図４に示される姿勢検出装置の誤差修正
モデルは、次式のとおりである。ただし、簡単のため、
運動加速度成分は、加速度誤差修正部２２の働きにより
修正され、作用しないとする。

【００５１】

【数２１】

【００５２】

【００５３】

【数２２】

【数２３】 である。

【００５４】

【００５５】

【数２４】

【００５６】

【数２５】

【００５７】

【数２６】 である。

【００５８】

【数２７】 のとき、数２１の右辺には、ジャイロバイアス成分Ｂｉ
の影響がなくなる。

【００５９】

【００６０】ジャイロバイアス演算部２５とジャイロバ
イアス修正部２６の動作を述べると以下となる。

【００６１】

【００６２】

【発明が解決しようとする課題】上述の従来の装置は、
運動加速度により生じる姿勢角誤差を防ぐため、Ｘ速度
計１１Ｖを用意し、その出力信号であるＸ速度信号ＶＸ
と方位角信号ＹＡＷとを加速度誤差修正部２２に入力す
ることで、対策を施している。しかしながら航行体が、
図７に示す様に旋回運動を行うある時点において、その
進行方向速度ＶＸＢと実際の軌跡速度ＶＰとが一致しな
くなる。この理由は、旋回時に生じる遠心力を航行体が
支持できなくなるために起る。

【００６３】図７の記号を説明すると（ここで（＋）Ｚ
軸方向は、紙面に対して下向きとする）、 Ｏ−ＸＬＹＬは、局地（又は、ローカル）座標系、添字
Ｌ Ｏ−ＸＢＹＢは、機体（又は、ボディ）座標系、添字Ｂ ＹＡＷは、ＣＴＭ演算部の機体座標系の方位角 ＹＡＷ′は、実際の航行体の方位角、又は軌跡角 δは、横すべり角 又、仮定として遠心力、向心力は、軌跡速度ＶＰの直交
方向に働くとし、さらに、上記Ｘ速度信号ＶＸは、機体
座標系のＸ軸速度ＶＸＢを検出するものとして扱う。

【００６４】この状況において、機体座標系の速度は、
Ｘ軸速度ＶＸＢ及びＹ軸速度ＶＹＢが生じ、その成分は
近似的に軌跡速度ＶＰと横すべり角δで表わされる。す
なわち、

【数２８】ＶＸＢ＝ＶＰｃｏｓδ ＶＹＢ＝−ＶＰｓｉｎδ である。

【００６５】所が、上記の加速度誤差修正部２２におい
て、Ｘ軸速度を検出したＸ軸速度信号ＶＸのみ（すなわ
ち、Ｙ軸速度ＶＹＢが検出できない）用いれているた
め、横すべり角δにより生じたＹ軸速度ＶＹＢによる加
速度成分の修正ができないのでそれによる姿勢角誤差
（いわゆる、横すべり角誤差）が生じる。

【００６６】従って、本発明は、航行体の旋回運動時に
於ても従来のように、Ｘ軸速度信号のみで横すべり角誤
差を生じない、すなわち、いかなる加速度運動において
も、加速度誤差を生じない、かつ、横すべり角の成分を
外部に出力できるストラップダウン方式の姿勢検出装置
を提供するものである。

【００６７】

【課題を解決するための手段】本発明のストラップダウ
ン方式の姿勢検出装置は、例えば、図１、図２に示す如
く、右手直交３軸座標系Ｏ−ＸＹＺと一致する取付軸１
０において、そのＸ軸には、Ｘ軸方向の加速度及び速度
をそれぞれ検出するＸ加速度計１１Ａ及びＸ速度計１１
ＶとＸ軸まわりの角速度を検出するＸジャイロ１１Ｇと
が取付けられ、Ｙ軸には、Ｙ軸方向の加速度を検出する
Ｙ加速度計１２ＡとＹ軸まわりの角速度を検出するＹジ
ャイロ１２Ｇとが取付けられ、Ｚ軸には、Ｚ軸方向の加
速度を検出するＺ加速度計１３ＡとＺ軸まわりの角速度
を検出するＺジャイロ１３Ｇと重力加速度方向の垂直軸
まわりの方位角を検出する磁気方位検出器１３Ｙとが取
付けられたセンサブロック部１と、このセンサブロック
部１のＸ，Ｙ，Ｚの加速度計１１Ａ，１２Ａ，１３Ａか
ら出力されるＸ，Ｙ，Ｚ加速度計、そのＸ，Ｙ，Ｚジャ
イロ１１Ｇ，１２Ｇ，１３Ｇから出力されるＸ，Ｙ，Ｚ
ジャイロ信号、及び磁気方位検出器１３Ｙから出力され
る方位信号を入力し、姿勢角であるロール角信号、ピッ
チ角信号、方位角信号及び横すべり角信号を演算して、
出力する演算部２′とにより構成されたものである。

【００６８】また、本発明のストラップダウン方式の姿
勢検出装置は、例えば図１、図２に示す如く上述におい
て、演算部２′に、局地座標系のＸ，Ｙ軸誤差に含まれ
る運動加速度成分を検出演算し、その情報を出力する機
能、その情報を入力して、ＣＴＭ演算に運動加速度の影
響が作用しない様に演算する機能を、従来の姿勢検出装
置に新たに設けて、いかなる航行体の運動に対しても、
運動加速度により誤差が生じない姿勢検出装置である。

【００６９】

【作用】本発明によれば、局地座標系のＸ，Ｙ軸誤差に
含まれる運動加速度成分から、横すべり角成分を検出演
算し、その情報を用いて座標変換行列演算に運動加速度
の影響が作用しない様に演算するので、運動速度による
誤差が生じず、かつ横すべり角の情報を外部に出力でき
る。

【００７０】

【実施例】以下、図１、図２及び図３を参照して、本発
明ストラップダウン方式の姿勢検出装置の一実施例を説
明する。この図１及び図２において、図４及び図５に対
応する部分には、同一符号を付し、これらの説明は省略
する。

【００７１】図１例において、図４の従来例と異なる点
は、演算部２′の発信信号に横すべり角成分δ′が加え
られたことであり、図２例において、図４の従来例と異
なる点は、演算部２′において、横すべり角演算部３０
と横すべり角修正部４０が新たに設けられたことであ
る。

【００７２】図１は、本例のストラップダウン方式の姿
勢検出装置の一例の全体を示すブロック図で、このスト
ラップダウン方式の姿勢検出装置はセンサブロック部１
と演算部２′より成る。図１のセンサブロック部１は、
右手直交３軸座標系と一致する取付軸１０において、Ｘ
軸には、Ｘ軸方向の加速度及び速度をそれぞれ検出する
Ｘ加速度計１１ＡとＸ速度計１１ＶとＸ軸まわりの角速
度を検出するＸジャイロ１１Ｇとが取付けられ、Ｙ軸に
は、Ｙ軸方向の加速度を検出するＹ加速度計１２ＡとＹ
軸まわりの角速度を検出するＹジャイロ１２Ｇとが取付
けられ、Ｚ軸には、Ｚ軸方向の加速度を検出するＺ加速
度計１３ＡとＺ軸まわりの角速度を検出するＺジャイロ
１３Ｇと重力加速度方向の垂直軸まわりの方位角を検出
する磁気方位検出器１３Ｙとが取付けられている。

【００７３】このセンサブロック部１のこの各３軸の加
速度計１１Ａ，１２Ａ，１３Ａ，３軸のジャイロ１１
Ｇ，１２Ｇ，１３Ｇ、磁気方位検出器１３Ｙ及びＸ軸速
度計１１Ｖからの出力信号を、演算部２′に入力する。
その演算部２′において、この入力信号を用いて演算し
て、センサブロック部１の取付軸１０のロール角信号Ｒ
ＯＬ、ピッチ角信号ＰＩＴ及び方位角信号ＹＡＷを姿勢
角として出力すると共に、横すべり角成分δ′もまた出
力する。

【００７４】本例のより詳細な動作について、図２及び
図３を用いて説明する。図２は、本例の演算部２′のブ
ロック図である。この演算部２′において、ＣＴＭ演算
部２０、水平誤差演算部２１、加速度誤差修正部２２、
方位誤差演算部２３、修正演算部２４、ジャイロバイア
ス演算部２５及びジャイロバイアス修正部２６の動作
は、図５と同様なので省略し、新たに設けられた横すべ
り角演算部３０及び横すべり角修正部４０について説明
する。

【００７５】この横すべり角による姿勢角誤差は、上記
した様に、航行体の実際の軌跡角ＹＡＷ′と方位角ＹＡ
Ｗとが、横すべり角δだけ回転しているため、加速度誤
差修正部２２における加速度誤差修正が正しく行なわれ
ないために生じる。従って、加速度誤差修正部２２の方
位角の入力信号が、軌跡角ＹＡＷ′を用いれば、横すべ
り角による姿勢角誤差は生じない。

【００７６】図３を参照して説明する。横すべり角演算
部３０は、Ｘ軸速度信号ＶＸ、方位角信号ＹＡＷ、水平
誤差演算部２１の出力であるＸ，Ｙ軸姿勢角誤差ＥＲＲ
Ｘ′，ＥＲＲＹ′及びＣＴＭ演算部２０の出力である３
行３列の行列信号ＣＴＭＳを入力として、横すべり角成
分β′を演算し、出力し、加速度演算部３１、加速度誤
差検出部３２及び座標変換演算部３３により成る。

【００７７】加速度演算部３１は、Ｘ軸速度信号ＶＸ
と、方位角信号ＹＡＷを用いて、局地座標系における修
正するＸ，Ｙ軸加速度成分ＡＸＬ′，ＡＹＬ′をそれぞ
れ、演算し、出力する。加速度誤差検出部３２には、
Ｘ，Ｙ軸姿勢角誤差ＥＲＲＸ′，ＥＲＲＹ′とＸ，Ｙ軸
加速度成分ＡＸＬ′，ＡＹＬ′とをそれぞれ対応する軸
ごとに差し引くことで、Ｘ，Ｙ軸の修正できない加速度
成分、すなわち、すべり角により加速度成分による加速
度誤差εＡＬＸ，εＡＬＹを検出して、出力する。

【００７８】座標変換演算部３３は、Ｘ，Ｙ軸加速度誤
差εＡＸＬ，εＡＹＬ及び行列信号ＣＴＭＳを用いて、
横すべり角成分を演算し、出力する。そして、横すべり
角修正部４０は、機体座標系の方位角信号ＹＡＷ及び上
記横すべり角成分δ′を入力として、両者を加算するこ
とで実際の航行体の方位角、すなわち軌跡角ＹＡＷ′を
演算し、出力する。軌跡角ＹＡＷ′を加速度誤差修正部
２２の方位角の入力として用いることで、すべり角によ
る加速度成分を誤差修正モデル（数２１）から取り除く
ことができる。

【００７９】次に、数式を用いてより詳細に説明する。
簡単のために、航行体の運動が一定旋回（速度、角速度
とも一定とする）として、この際生じる横すべり角も一
定として以降扱う。水平誤差演算部２１の出力である
Ｘ，Ｙ軸誤差ＥＲＲＸ′，ＥＲＲＹ′に含まれるＸ，Ｙ
軸運動加速度成分ＡＸＬ，ＡＹＬは、図７を参照して、
次式となる。

【００８０】

【数２９】

【００８１】いま、運動加速度成分以外の誤差はないと
すれば、

【数３０】ＥＲＲＸ′＝ＡＸＬ ＥＲＲＹ′＝ＡＹＬ となる。

【００８２】一方、横すべり演算部３０の加速度演算部
３１において、演算されるＸ，Ｙ軸運動加速度成分ＡＸ
Ｌ′，ＡＹＬ′は、次式となる。

【００８３】

【数３１】

【００８４】よって、横すべり演算部３０の加速度誤差
検出部３２において、演算される加速度成分の残差は数
２９，数３０及び数３１そして数２８を用いて次式とな
る。

【００８５】

【数３２】

【００８６】

【数３３】

【００８７】ここで、δは、横すべり角 ＶＰは、軌跡速度 ＶＸＢは、機体のＸ軸速度で、

【数３４】ＶＸＢ＝ＶＸ ωは、旋回角速度 ＹＡＷ′は、軌跡角 さらに、近似計算のため次の仮定を行う。

【００８８】

【数３５】

【００８９】次に、横すべり角演算部３０の座標変換演
算部３３において、実際の横すべり角成分δ′が次式の
ように展開されている。すなわち、

【数３６】 ここで、ＣＴＭＳは、簡単のために、方位角信号のみと
している。

【００９０】従って、計算される横すべり角成分β′
は、数３６のＹ軸の値、既知のＸ軸速度信号ＶＸＢ、角
速度ωを用いて、次式により演算される。すなわち、

【００９１】

【数３７】

【００９２】尚、ここで、横すべり角δは、実際の値で
あるので、演算された横すべり角は、横すべり角成分
δ′としている。よって、数３６，数３７より

【数３８】δ′＝δ そして、横すべり角修正部４０において、軌跡角ＹＡ
Ｗ′は、

【数３９】ＹＡＷ′＝ＹＡＷ−δ′ により、演算される。

【００９３】加速度誤差修正部２２において、軌跡角Ｙ
ＡＷ′とＸ軸速度信号ＶＸＢを用いると、次のようにな
る。すなわち、数３１のＹＡＷをＹＡＷ′に置き換え
て、加速度成分の残差を計算すると、

【数４０】

【００９４】

【数４１】

【００９５】よって、軌跡角ＹＡＷ′を用いると、横す
べり角δによる運動加速度成分を取り除くことができ、
かつ、常時横すべり角成分δ′を計算しているので、そ
れを外部に出力できる。

【００９６】尚、本発明は上述実施例に限ることなく本
発明の要旨を逸脱することなく、その他種々の構成が採
り得ることは勿論である。

【００９７】

【発明の効果】本発明によれば、航行体が横すべり角も
生じる運動においても、運動加速度成分による姿勢角誤
差を生じさせない姿勢検出装置が実現でき、その精度の
飛躍的向上を図ることができる。

【００９８】また、この横すべり角の演算及びその修正
の方法は、機器の追加変更を伴わず、比較的簡単な演算
を施すだけの構成で良い利益がある。

【００９９】また本発明によれば局地座標系のＸ，Ｙ軸
誤差に含まれる運動加速度成分から横すべり角成分を検
出演算し、その情報を用いて座標変換行列演算に運動加
速度の影響が作用しない様に演算するので運動加速度に
にる誤差が生じず、かつ横すべり角の情報を外部に出力
できる利益がある。

【図面の簡単な説明】

【図１】本発明ストラップダウン方式の姿勢検出装置の
一実施例を示す構成図である。

【図２】本発明の演算部２′の例を示す構成図である。

【図３】図２の横すべり角演算部３０と横すべり角修正
部４０との説明に供する線図である。

【図４】従来の姿勢検出装置の例を示す構成図である。

【図５】従来の演算部２の例を示す構成図である。

【図６】図５のジャイロバイアス演算部２５とジャイロ
バイアス修正部２６との説明に供する線図である。

【図７】横すべり角の発生を説明する線図である。

【符号の説明】 １ センサブロック部 ２′ 演算部 １１Ｖ 速度計 １１Ａ Ｘ加速度計 １１Ｇ Ｘジャイロ １２Ａ Ｙ加速度計 １２Ｇ Ｙジャイロ １３Ａ Ｚ加速度計 １３Ｇ Ｚジャイロ １３Ｙ 磁気方位検出器 ２０ ＣＴＭ演算部 ２１ 水平誤差演算部 ２２ 加速度誤差修正部 ２３ 方位誤差演算部 ２４ 修正量演算部 ２５ ジャイロバイアス演算部 ２６ ジャイロバイアス修正部 ３０ 横すべり角演算部 ３１ 加速度演算部 ３２ 加速度誤差検出部 ３３ 座標変換演算部 ４０ 横すべり角修正部

───────────────────────────────────────────────────── フロントページの続き (72)発明者 山本 貫志 東京都大田区南蒲田２丁目16番46号 株式 会社トキメック内

Claims (4)

【特許請求の範囲】
1. 【請求項１】 右手直交３軸座標系Ｏ−ＸＹＺと一致す
   る取付軸において、そのＸ軸には、Ｘ軸方向の速度及び
   加速度をそれぞれ検出するＸ速度計及びＸ加速度計とＸ
   軸まわりの角速度を検出するＸジャイロとが取付けら
   れ、そのＹ軸には、Ｙ軸方向の加速度を検出するＹ加速
   度計とＹ軸まわりの角速度を検出するＹジャイロとが取
   付けられ、そのＺ軸には、Ｚ軸方向の加速度を検出する
   Ｚ加速度計とＺ軸まわりの角速度を検出するＺジャイロ
   と重力加速度方向の軸まわりの方位角を検出する磁気方
   位検出器が取付けられたセンサブロック部と、 該センサブロック部の上記Ｘ速度計から出力されるＸ速
   度信号、上記Ｘ，Ｙ及びＺ加速度計から出力されるＸ，
   Ｙ及びＺ加速度信号、上記Ｘ，Ｙ及びＺジャイロから出
   力されるＸ，Ｙ及びＺジャイロ信号、そして上記磁気方
   位検出器から出力される方位信号を入力とし、ロール角
   信号、ピッチ角信号方位角信号及び横すべり角成分を演
   算して、出力する演算部とより構成されることを特徴と
   するストラップダウン方式の姿勢検出装置。
2. 【請求項２】 請求項１記載のストラップダウン方式の
   姿勢検出装置において、上記演算部に、局地座標系の
   Ｘ，Ｙ及びＺ軸誤差を検出演算し、その情報を用いて姿
   勢角誤差を修正する機能を有することで、上記ロール
   角、ピッチ角及び方位角信号に誤差を含まないことを特
   徴とするストラップダウン方式の姿勢検出装置。
3. 【請求項３】 請求項１記載のストラップダウン方式の
   姿勢検出装置において、上記演算部に、上記Ｘ，Ｙ及び
   Ｚジャイロ信号に含まれるＸ，Ｙ及びＺジャイロバイア
   ス成分を局地座標系のＸ，Ｙ及びＺ軸誤差を用いて演算
   し、それらを修正する機能を有することで、上記３つの
   ジャイロバイアス成分による姿勢角誤差を生じさせない
   ことを特徴とするストップダウン方式の姿勢検出装置。
4. 【請求項４】 請求項１記載のストラップダウン方式の
   姿勢検出装置において、上記演算部に、回転運動時に伴
   ない生じる横すべり角による加速度を含む運動加速度成
   分を局地座標系のＸ，Ｙ及びＺ軸誤差、座標行列及びＸ
   速度計を用いて演算し、外部に出力する機能を有すると
   ともに、上記局地座標系のＸ．Ｙ及びＺ軸に含まれる運
   動加速度成分を修正する機能を有することで、運動加速
   度による誤差を修正することを特徴とするストラップダ
   ウン方式の姿勢検出装置。