JP3182185B2 - 動揺角検出器 - Google Patents

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、レートセンサ及び傾斜
センサの出力を相補的に合成し、動揺角を検知出力する
動揺角検出器の改良に関する。

【０００２】

【従来の技術】船舶等の移動体に搭載され移動体の動揺
角（ロール、ピッチ等）を検出する動揺角検出器として
は、レートセンサと傾斜センサを用い、これらの出力を
相補的合成フィルタにより合成して、所望周波数領域で
平坦な周波数特性を実現する構成がある。

【０００３】図１２には、一従来例に係る動揺角検出器
の構成が示されている。この図に示される動揺角検出器
は、角速度センサ１０、傾斜センサ１２、相補的合成フ
ィルタ１４から構成されている。

【０００４】角速度センサ１０はピエゾ型レートセンサ
等として構成されるものであり、図１３に示されるよう
に伝達関数 Ｇ₁₀（ｓ）＝Ｋ₁ ・ｓ（Ｋ₁ ：定数） を有している。すなわち、加わる動揺角θ_i （ｓ）の微
分（角速度）を出力するセンサである。図１３において
Ｇ₁₁（ｓ）及びｄ₀ （ｓ）で示されるのは、それぞれＬ
ＰＦ特性を有する寄生要素並びにオフセットとそのドリ
フトであり、以下のように表される。

【０００５】

【数１】 ただし、ω₁ 、ζ₁ は角速度センサ１０の２次遅れ係数
でありそれぞれ遮断周波数及びダンピング係数を表して
おり、Ｇ₁₁（ｓ）の式は寄生要素を２次ＬＰＦとしてモ
デル化している。ｄ₀ は、角速度センサ１０のオフセッ
ト電圧である。また、傾斜センサ１２はインクリノメー
タとよばれるものであり、図１３に示されるように伝達
関数 Ｇ₂₀（ｓ）＝Ｋ₂ （Ｋ₂ ：定数） を有している。図１３においてＧ₂₁（ｓ）及びＧ
₂₂（ｓ）で示されるのは、それぞれＬＰＦ特性を有する
寄生要素並びに加速度の影響であって、以下のように表
される。

【０００６】

【数２】 ただし、ω₂ 、ζ₂ は傾斜センサ１２の２次遅れ係数で
ありそれぞれ遮断周波数及びダンピング係数を表してお
り、Ｇ₂₁（ｓ）の式は寄生要素を２次ＬＰＦとしてモデ
ル化している。Ｌは移動体の動揺中心から傾斜センサ１
２までの距離を表しており、ｇは重力加速度である。

【０００７】これらの式に基づき、オフセットや加速度
の影響を含めた角速度センサ１０及び傾斜センサ１２の
伝達関数を表現すると、それぞれ次のように表される。

【０００８】Ｇ₁₀（ｓ）Ｇ₁₁（ｓ）＋ｄ₀ （ｓ） Ｇ₂₀（ｓ）Ｇ₂₁（ｓ）＋Ｇ₂₂（ｓ） 相補的合成フィルタ１４は、角速度センサ１０及び傾斜
センサ１２の出力を相補的に、すなわち必要な周波数領
域で出力θ_o （ｓ）に周波数特性が現れないように、合
成するフィルタである。

【０００９】今、オフセットや寄生要素を考えないこと
とすると、角速度センサ１０の伝達関数はＧ₁₀（ｓ）＝
Ｋ₁ ・ｓで表される。また、加速度の影響や寄生要素を
考えないこととすると、傾斜センサ１２の伝達関数はＧ
₂₀（ｓ）＝Ｋ₂ で表される。両者の出力を相補的に合成
するためには、原理的には、 Ｇ_rate ⁽⁰⁾ （ｓ）＋Ｇ_incl ⁽⁰⁾ （ｓ）＝１ という関係を成立させれば良い。ここに、Ｇ
_rate ⁽⁰⁾ （ｓ）は角速度センサ１０及び相補フィルタ１
６を含む伝達関数であり、Ｇ_incl ⁽⁰⁾ （ｓ）は傾斜セン
サ１２及び相補フィルタ１８を含む伝達関数である。

【００１０】相補フィルタ１６及び１８は、それぞれ角
速度センサ１０又は傾斜センサ１２に縦続接続されてお
り、加算器２０は相補フィルタ１６及び１８の出力を加
算して検出結果θ_o （ｓ）として出力する。相補フィル
タ１６の伝達関数Ｆ_rate ⁽⁰⁾（ｓ）及び相補フィルタ１
８の伝達関数Ｆ_incl ⁽⁰⁾ （ｓ）は、具体的には Ｆ_rate ⁽⁰⁾ （ｓ）＝Ｋ_a ／（ｓ＋ω_a ） Ｆ_incl ⁽⁰⁾ （ｓ）＝Ｋ_b ／（ｓ＋ω_a ） と設定される。ここに、Ｋ_a ＝１／Ｋ₁ であり、Ｋ_b ＝
ω_a ／Ｋ₂ とした場合、 Ｋ_a ／（ｓ＋ω_a ）・Ｋ₁ ・ｓ＋Ｋ_b ／（ｓ＋ω_a ）・Ｋ₂ ＝１ となり、相補的合成が実現されることがわかる。

【００１１】

【発明が解決しようとする課題】しかし、通常の傾斜セ
ンサは、重力方向の基準を得るための振り子（pendulu
m）を有している。このため、加速度の影響による誤差
（上の従来例ではＧ₂₂（ｓ）に起因する誤差）が大きく
なる。また、角速度センサ（レートセンサ）はオフセッ
トを有しており、またその温度ドリフトが大きい（上の
従来例ではｄ₀ （ｓ））。レートセンサのオフセットに
より生じる動揺角出力の誤差（オフセット誤差）は、最
終値定理より、ｓ・ｄ₀ （ｓ）・Ｇ₁₀（ｓ）のｓ→０の
極限値としてＤＲ_O ⁽⁰⁾ ＝ｄ₀ ／（Ｋ₁ ・ω_a ）とな
る。現在市販されている安価な振動ジャイロ型の角速度
センサは、Ｋ₁ ＝１．２６［Ｖ／ｒａｄ／ｓｅｃ］、ｄ
₀ ＝−０．２〜０．２［Ｖ］（温度による）程度の特性
であるから、恒温槽内で使用する等の場合を除き、実用
上ＤＲ_O ⁽⁰⁾ が問題となる。

【００１２】本発明は、このような問題点を解決するこ
とを課題としてなされたものであり、レートセンサ及び
傾斜センサを用いて安価かつより高精度の動揺角検出器
を実現し、インマルサット船舶地球局のアンテナ制御等
に好適に採用可能とすることを目的とする。

【００１３】

【課題を解決するための手段】このような目的を達成す
るために、本発明は、レートセンサの出力を瀘波する第
１の相補フィルタが帰還係数Ｋ_b の帰還ループを含み、
動揺の周波数を検出する手段及び検出結果に応じて補正
係数α（α≦１）を決定して第２の相補フィルタの伝達
関数の分子に含まれる微分項をこの補正係数αにより補
正する制御手段を含むパラメータ適応制御手段を備え、
動揺の周波数に応じ補正係数αの適応制御を行うことを
特徴とする。

【００１４】また、請求項２は、さらに、パラメータ適
応制御手段が動揺の振幅を検出する手段を備え、制御手
段が検出される動揺の振幅に応じて帰還係数Ｋ_b を決定
して第１及び第２の相補フィルタに供給し、動揺の振幅
に応じて帰還係数Ｋ_b の適応制御を行うことを特徴とす
る。

【００１５】

【作用】本発明においては、レートセンサの出力を瀘波
する第１の相補フィルタに帰還係数Ｋ_b の帰還ループが
設けられる。このような帰還ループが設けられると、第
１の相補フィルタの伝達関数の分母に帰還係数Ｋ_b が現
れ、これに伴い、レートセンサのオフセット誤差を表す
式の分母にも帰還係数Ｋ_b が現れる。従って、帰還係数
Ｋ_b を大きく設定することにより、オフセット誤差を低
減することが可能になる。

【００１６】また、本発明においては、動揺の周波数が
検出され、検出結果に応じて補正係数αが適応制御され
る。この補正係数αは、第２の相補フィルタの伝達関数
の分子に現れる微分項を補正する係数である。第２の相
補フィルタの伝達関数の分子に微分項が現れるのは、第
１及び第２の相補フィルタが相補性を満たすように設定
されていること、及び第１の相補フィルタに帰還ループ
が設けられていることによるものであるが、この微分項
は、動揺誤差、すなわち傾斜センサに対する加速度の影
響による誤差を増大させるものである。本発明では、こ
の微分項の影響が補正係数αにより抑制され、動揺誤差
が低減される。

【００１７】また、請求項２においては、パラメータ適
応制御手段により動揺の振幅が検出され、これに応じて
帰還係数Ｋ_b が適応制御される。先に述べたように、帰
還係数Ｋ_b を大きく設定することによりオフセット誤差
を低減することが可能であるが、一般に動揺誤差も大き
くなる。そこで、請求項２においては、補正係数αと共
に帰還係数Ｋ_b が適応制御され、動揺誤差の低減がより
顕著になる。

【００１８】

【実施例】以下、本発明の好適な実施例について図面に
基づき説明する。なお、図１２及び１３に示される従来
例と同様の構成には同一の符号を付し説明を省略する。

【００１９】（１）第１実施例 図１には、本発明の第１実施例に係る動揺角検出器の構
成が示されている。また、図２にはその特徴的構成であ
るパラメータ適応制御手段２２の構成が、図３には本実
施例の伝達関数によるブロック図が、それぞれ示されて
いる。

【００２０】図１に示されるように、本実施例は角速度
センサ１０、傾斜センサ１２、パラメータ適応型相補的
合成フィルタ２４及びパラメータ適応制御手段２２から
構成されている。

【００２１】（１．１）帰還ループによるオフセット誤
差の低減 この実施例では、パラメータ適応型相補的合成フィルタ
２４は、帰還ループ付相補フィルタ２６、相補フィルタ
２８及び加算器２０から構成されている。相補フィルタ
２６及び２８はそれぞれ角速度センサ１０又は傾斜セン
サ１２の後段に接続されており、加算器２０は両フィル
タ２６及び２８の出力を加算する。

【００２２】相補的合成フィルタ２４の構成が従来例の
それと異なる点は、第１に、相補フィルタ２６が帰還ル
ープを含んでおり（図３参照）、オフセット誤差ＤＲ₀
⁽¹⁾を低減可能な構成となっていることである。

【００２３】図３に示されるように、帰還ループの伝達
関数を Ｈ_b （ｓ）＝Ｋ_b ω_b ／（ｓ＋ω_b ） とすると、相補フィルタ２６の伝達関数Ｆ
_rate ⁽¹⁾ （ｓ）は、従来のフィルタ１６に相当する伝達
関数 Ｇ₂ （ｓ）＝Ｋ_a ／（ｓ＋ω_a ） とこの帰還ループの伝達関数Ｈ_b （ｓ）の伝達関数の合
成値

【数３】 となる。なお、ω_b は帰還ループの遮断周波数、Ｋ_b は
帰還ループの帰還係数である。後述するように、Ｋ_b は
パラメータ適応制御手段２２により動揺の状況に応じた
制御を受ける。

【００２４】この伝達関数Ｆ_rate ⁽¹⁾ （ｓ）は、相補フ
ィルタ２６が２次のバンドパスフィルタとして機能する
ことを表している。ただし、本発明では、相補フィルタ
２６を２次以上に設定することもできる。

【００２５】角速度センサ１０のオフセットに起因する
出力θ_o （ｓ）の誤差、すなわちオフセット誤差ＤＲ₀
⁽¹⁾ は、最終値定理より、ｓ・ｄ₀ （ｓ）・Ｆ_rate ⁽¹⁾
（ｓ）のｓ→０の極限値として求められ、次の式で表さ
れる値となる。

【００２６】ＤＲ_O ⁽¹⁾ ＝ｄ₀ ／（Ｋ₁ ω_a ＋Ｋ_b ） この式から、帰還ループの帰還係数Ｋ_b を大きくする
と、オフセット誤差ＤＲ_O ⁽¹⁾ が小さくなることがわか
る。すなわち、本実施例によれば、相補フィルタ２６に
帰還ループを設けたことにより、帰還係数Ｋ_b の設定で
オフセット誤差ＤＲ_O ⁽¹⁾ を低減できる。

【００２７】（１．２）補正係数αによる動揺誤差の低
減 さらに、本実施例における相補フィルタ２８の伝達関数
は、従来例における相補フィルタ１８のそれと異なって
いる。

【００２８】まず、所定周波数領域で相補性を満足させ
るためには、従来例と同様、次の関係を満たす必要があ
る。

【００２９】 Ｋ₁ ・ｓ・Ｆ_rate ⁽¹⁾ （ｓ）＋Ｋ₂ ・Ｆ_incl ⁽¹⁾ （ｓ）＝１ Ｆ_incl ⁽¹⁾ （ｓ）：相補フィルタ２８の伝達関数 ただし、ここでは、角速度センサ１０及び傾斜センサ１
２における寄生要素や角速度センサ１０のドリフト、傾
斜センサ１２における加速度の影響を無視している。

【００３０】一方、相補フィルタ２６の伝達関数Ｆ_rate
⁽¹⁾ （ｓ）が先に記したような式で表されることから、
上式の変形で相補フィルタ２８の伝達関数Ｆ
_incl ⁽¹⁾ （ｓ）が次のように求まる。

【００３１】

【数４】 この式の分子には、ラプラス演算子ｓが現れており、微
分として作用する。この項により、いわゆる動揺誤差が
発生する。

【００３２】動揺誤差とは傾斜センサ１２への加速度の
影響により出力θ_o （ｓ）に現れる誤差である。動揺が
短周期でかつ設置高さＬが大きい場合、微分項ω_a ・ｓ
により角速度の影響Ｇ₂₂（ｓ）が強調され、誤差が大き
くなると考えられる。

【００３３】そこで、本実施例では、微分項の寄与分を
小さくするため相補フィルタ２８の伝達関数Ｆ_incl ⁽¹⁾
（ｓ）に次のように補正係数αを導入し、微分項ω_a ・
ｓに補正係数α＜１を乗じることにより動揺誤差を低減
している。

【００３４】

【数５】 （１．３）補正係数α及び帰還係数Ｋ_b の適応制御 以上のように、帰還係数Ｋ_b を大きくすることによりオ
フセット誤差を低減し、補正係数αの導入により動揺誤
差を低減できる。しかし、動揺誤差は、帰還係数Ｋ_b を
大きくすると補正係数αにかかわらず大きくなる。従っ
て、オフセット誤差及び動揺誤差を共に低減するために
は、補正係数α及び帰還係数Ｋ_b の適応制御を行う必要
がある。

【００３５】この適応制御のため、本実施例は、パラメ
ータ適応制御手段２２を備えている。パラメータ適応制
御手段２２は、パラメータ適応型相補的合成フィルタ２
４の出力θ_o （ｓ）から動揺の周波数及び振幅を検出
し、検出結果に基づきパラメータ制御信号（α，Ｋ_b ）
をパラメータ適応型相補的合成フィルタ２４に出力す
る。パラメータ適応型相補的合成フィルタ２４において
は、このパラメータ制御信号（α，Ｋ_b ）に応じ、パラ
メータ（α，Ｋ_b ）が切り換わる。

【００３６】図２に示されるように、本実施例における
パラメータ適応制御手段２２は、動揺周波数検出手段３
０、動揺振幅検出手段３２及びパラメータ（α，Ｋ_b ）
制御手段３４から構成されている。動揺周波数検出手段
３０及び動揺振幅検出手段３２は、出力θ_o （ｓ）から
それぞれ動揺の周波数及び振幅を検出し、検出結果をパ
ラメータ（α，Ｋ_b ）制御手段３４に出力する。周波数
及び振幅の検出は、例えばＦＦＴ又はＤＦＴの手法を用
いて行う。なお、入力される信号の周波数及び振幅をＦ
ＦＴ又はＤＦＴによって求める手法は公知のアルゴリズ
ムであり、例えば文献“The Fast Fourier Transfor
m”、Brigham 、Prentice Hall(1974) に示されてい
る。

【００３７】パラメータ（α，Ｋ_b ）制御手段３４は、
動揺周波数検出手段３０及び動揺振幅検出手段３２によ
り検出される動揺の周波数及び振幅に応じ、パラメータ
（α，Ｋ_b ）が対応する値となるよう制御する。

【００３８】この制御によって、オフセット誤差及び動
揺誤差を共に低減することができる。なお、補正係数α
の適応制御により補正係数αが１に比べ小さくなった場
合低周波における相補性が一部崩れる。また、これに伴
い、低周波での誤差が増大する可能性があるが、これ
は、補正係数αによる誤差低減に比べ無視できる程度に
抑圧できる。

【００３９】また、実際の場合は、高域（船舶に搭載し
その動揺角を検出する場合１［Ｈｚ］前後又はそれ以
上）における角速度センサ１０の位相遅れ等により相補
性は乱れる。したがって、用途に応じて角速度センサ１
０の特性を吟味しなければならない。本実施例では高域
においては相補性がその用途に必要な範囲でほぼ満たさ
れていると仮定し、以下の説明でも引き続き相補フィル
タ等の用語を用い、本実施例のモデルを相補性モデル
（reciprocal model）とよぶこととする。なお、α＝１
の場合を完全相補性モデル（complete reciprocal mode
l ）とよび、α≠１の場合を不完全相補性モデル（inco
mplete reciprocal model ）と呼ぶこととする。

【００４０】（１．４）適応制御の具体的内容 補正係数α及び帰還係数Ｋ_b の適応制御を行う上で、こ
れら補正係数α、帰還係数Ｋ_b の変化により誤差がどの
様に変化するかを知る必要がある。すなわち、誤差が最
小となるよう、シミュレーション等を行って補正係数α
及び帰還係数Ｋ_b の適応制御の内容（何段階で切り換え
るか、値は、等）を決定する。

【００４１】図４〜６には、動揺誤差のシミュレーショ
ン結果が示されている。これらの図は、動揺振幅２０
［ｄｅｇ］、動揺周期１〜３３［ｓｅｃ］の正弦波につ
いて、動揺誤差を求めたものである。帰還係数Ｋ_b は、
５．０（図４）、１０．０（図５）、１５．０（図６）
の３通りを考えており、傾斜センサ１２の設置高さＬは
２０［ｍ］、ｆ₁ ＝ω₁ ／２π＝７．０［Ｈｚ］、ζ₁
＝１．０、ｆ₁ ＝ω₂ ／２π＝１．０［Ｈｚ］、ζ₂ ＝
１．０とし、補正係数αとして−０．５、０、０．５、
１の４段階を設定している。

【００４２】これらの図からわかるように、動揺周期が
短い場合には補正係数αが１に比べより小さい方が動揺
誤差が少なく、１により近い方が動揺誤差が大きい。ま
た、帰還係数Ｋ_b が大きいと動揺誤差が大きい。

【００４３】さらに、図７には、伝達関数Ｆ
_rate ⁽¹⁾ （ｓ）のランプ応答（角速度センサ１０のオフ
セットのドリフトによる誤差、すなわちドリフト誤差）
が示されている。用いたランプ入力は、０［Ｖ］からス
タートし１０［ｍｉｎ］で５０［ｍＶ］（約２［ｄｅｇ
／ｓｅｃ］の角速度に相当）に達するランプ入力であ
り、帰還係数Ｋ_b が５．０の場合、１０．０の場合、１
５．０の場合の３通りについてランプ応答を求めてい
る。この図から、帰還係数Ｋ_b を小さくすると、ドリフ
ト誤差を小さくできることがわかる。

【００４４】このようなシミュレーション結果から、補
正係数α及び帰還係数Ｋ_b の適応制御の内容を決定する
と、例えば次のようになる。

【００４５】図８には、適応制御の一例が示されてい
る。この図の例では、パラメータ（α，Ｋ_b ）制御手段
３４は、動揺周波数検出手段３０により検出される動揺
の周波数（＝１／動揺の周期）に応じ、補正係数αを次
の２段階で切り換え制御する。

【００４６】 動揺の周期≧１２［ｓｅｃ］ α＝０．５ 動揺の周期＜１２［ｓｅｃ］ α＝０ また、パラメータ（α，Ｋ_b ）制御手段３４は、動揺振
幅検出手段３２により検出される動揺の振幅（ｒｍｓ
値）に応じ、帰還係数Ｋ_b を次の３段階で切り換え制御
する。

【００４７】 動揺の振幅＜５［ｄｅｇ］ Ｋ_b ＝１５．０ ５［ｄｅｇ］≦動揺の振幅＜１５［ｄｅｇ］ Ｋ_b ＝１０．０ 動揺の振幅≧１５［ｄｅｇ］ Ｋ_b ＝５．０ （１．５）詳細な伝達関数 帰還係数Ｋ_b 、遮断周波数ω_a 、ω_b 等の決定は、以上
の説明における簡略化された伝達関数式では足りず、よ
り詳細な式により時系列解析を行い決定することが必要
である。

【００４８】まず、角速度センサ１０及び相補フィルタ
２６をあわせた系統（以下、レートセンサ系という）の
伝達関数Ｇ_rate ⁽¹⁾ （ｓ）は、寄生要素の伝達関数Ｇ₁₁
（ｓ）を考慮した場合、 Ｇ_rate ⁽¹⁾ （ｓ）＝Ｇ₁₀（ｓ）Ｇ₁₁（ｓ）Ｆ_rate ⁽¹⁾ （ｓ） から、次の式で表される。

【００４９】

【数６】 また、寄生要素の伝達関数Ｇ₂₁（ｓ）や加速度Ｇ
₂₂（ｓ）を考慮すると、傾斜センサ１２及び相補フィル
タ２８をあわせた系統（以下、傾斜センサ系という）の
伝達関数 Ｇ_incl ⁽¹⁾ （ｓ）＝｛Ｇ₂₀（ｓ）Ｇ₂₁（ｓ）＋Ｇ₂₂（ｓ）｝Ｆ_incl ⁽¹⁾ （ｓ） は、次の式で表される。

【００５０】

【数７】 従って、図１の回路の総合の伝達関数Ｇ
_total ⁽¹⁾ （ｓ）は、次の式で表される。

【００５１】

【数８】 ただし、ｒ₁ ＝２ζ₁ ω₁ 、ｒ₂ ＝ω₁ ² 、ｐ₁ ＝２ζ
₂ ω₂ 、ｐ₂ ＝ω₂ ² 、ｂ₁ ＝ω_a ＋ω_b 、ｂ₂ ＝ω_b
（ω_a ＋Ｋ_a Ｋ_b ）、ａ₀ ＝αＫ_Lgｐ₂ ω_a 、ａ₁ ＝Ｋ
_Lgｐ₂ ｂ₂ 、ａ₂ ＝αｐ₂ ω_a 、ａ₃ ＝ｐ₂ ｂ₂ 、Ｋ_Lg
＝−Ｌ／ｇ である。

【００５２】（１．６）実施例の効果 このような構成を有する本実施例によれば、オフセット
誤差、ドリフト誤差、動揺誤差を低減できる。

【００５３】まず、オフセット誤差を従来例と比較した
場合、 従来例におけるオフセット誤差ＤＲ₀ ⁽⁰⁾＝０．１２６
［ｒａｄ］＝７．２４［ｄｅｇ］ 本実施例におけるオフセット誤差ＤＲ₀ ⁽¹⁾＝０．０１
０［ｒａｄ］＝０．５７［ｄｅｇ］ となり、著しくオフセット誤差が低減される。このデー
タは、 Ｋ₁ ＝１．２６［Ｖ／ｒａｄ／ｓｅｃ］ ｄ₀ ＝０．１［Ｖ］ ω_a ⁽⁰⁾ ＝２π×０．１［ｒａｄ］，ω_a ⁽¹⁾ ＝２π×
０．００２［ｒａｄ］ Ｋ_b ＝１０．０ とした場合のデータである。

【００５４】従って、本実施例では、帰還ループにより
オフセット誤差を低減でき、オフセット誤差の大きいレ
ートセンサを角速度センサ１０に用いることができる。

【００５５】また、ドリフト誤差、動揺誤差に関して
は、図４〜７のシミュレーション結果から明らかなよう
に、補正係数α及び帰還係数Ｋ_b の適応制御によってト
ータルで低減できる。

【００５６】（２）第２実施例 図９には、本発明の第２実施例に係る動揺角検出器の構
成が示されている。この実施例では、パラメータ適応制
御手段３６が出力θ_o から動揺の周波数及び振幅を検出
するのではなく、傾斜センサ１２の出力から検出してい
る。パラメータ適応制御手段３６の動作は第１実施例と
ほぼ同じである。この実施例でも、第１実施例と同様の
効果を得ることができる。

【００５７】（３）第３実施例 図１０には、本発明の第３実施例に係る動揺角検出器の
構成が示されている。この実施例では、パラメータ適応
制御手段３８が受信機から受信レベル信号を入力し、帰
還係数Ｋ_b 及び補正係数αを適応制御している。

【００５８】ここにいう受信機は、船舶に設置されたイ
ンマルサット船舶地球局等の受信機であり、この受信機
から出力される受信レベル信号は通信衛星から受信され
る信号のレベルを表している。パラメータ適応制御手段
３８は、受信レベル信号に基づきステップトラックを実
行する。

【００５９】すなわち、パラメータ適応制御手段３８は
微小値のステップトラック信号を発生させ、受信レベル
がより大きくなる方向にこの信号の符号（＋／−）を設
定し、（α，Ｋ_b ）制御信号として出力する。この信号
により、補正係数α及び帰還係数Ｋ_b の値が漸増・漸減
し、結果として誤差の小さい出力θ_o が得られる。

【００６０】（４）第４実施例 図１１には、本発明の第４実施例に係る動揺角検出器の
構成が示されている。この実施例では、相補フィルタ２
６及び２８がディジタルフィルタとして実現されてお
り、これに伴い角速度センサ１０及び傾斜センサ１２の
出力側にＡＤコンバータ４０及び４２がそれぞれ設けら
れている。また、角速度センサ１０の出力のオフセット
を補正するレジスタを設け、受信レベル信号によりステ
ップトラックを行っている。なお、パラメータ適応制御
手段４８は第１実施例とほぼ同様の構成である。

【００６１】（４．１）ディジタルフィルタによる実現
（implementation） この実施例のように相補フィルタ２６及び２８をディジ
タルフィルタとして構成する場合、パラメータ制御がや
りやすい（補正係数α及び帰還係数Ｋ_b の制御が比較的
容易である）。ディジタルフィルタに関しては、例えば
“Digital Signal Processing ”,A.V.Oppenheim,R.N.S
chafer,Prentice Hall(1975)等に示されている。本実施
例では、ディジタルフィルタによる実現を下記のように
双一次変換（Bilinear Transformation ）により行って
いる。

【００６２】まず、レートセンサ系の相補フィルタ２６
の実現に関して説明する。単位時間Ｔ（１ビット）の遅
延を表す演算子ｕ＝ｚ^-1を用い、ｓ＝ｈ（１−ｕ）／
（１＋ｕ）、ｈ＝２／Ｔによりラプラス演算子ｓを用い
た伝達関数Ｆ_rate ⁽¹⁾ （ｓ）を双一次変換すると、次の
式で表現される。

【００６３】Ｆ_rate ⁽¹⁾ （ｕ）＝（Ｈ₀ ・ｕ² ＋Ｈ₁ ・
ｕ＋Ｈ₂ ）／（Ｎ₀ ・ｕ² ＋Ｎ₁ ・ｕ＋Ｎ₂ ） ＝Ｚ（ｕ）／Ｒ（ｕ） Ｒ（ｕ）：相補フィルタ２６への入力系列 Ｚ（ｕ）：相補フィルタ２６からの出力系列 Ｈ₀ ＝Ｋ_a （−ｈ＋ω_a ） Ｈ₁ ＝２Ｋ_a ω_b Ｈ₂ ＝Ｋ_a （ｈ＋ω_b ） Ｎ₀ ＝ｈ² −ｂ₁ ｈ＋ｂ₂ Ｎ₁ ＝−２ｈ² ＋２ｂ₂ Ｎ₂ ＝ｈ² ＋ｂ₁ ｈ＋ｂ₂ この式において、Ｒ（ｕ）・ｕⁱ ＝Ｒ_-i、Ｚ（ｕ）・ｕ
ⁱ ＝Ｚ_-iと表すと、出力系列Ｚ（ｕ）は次の差分方程式
で表すことができる。

【００６４】 Ｚ（ｕ）＝Ｈ₀₀Ｒ_-2＋Ｈ₁₀Ｒ_-1＋Ｈ₂₀Ｒ（ｕ）−（Ｎ₀₀
Ｒ_-2＋Ｎ₁₀Ｒ_-1＋Ｎ₂₀Ｒ（ｕ）） Ｈ₀₀＝Ｈ₀ ／Ｎ₂ Ｈ₁₀＝Ｈ₁ ／Ｎ₂ Ｈ₂₀＝Ｈ₂ ／Ｎ₂ Ｎ₀₀＝Ｎ₀ ／Ｎ₂ Ｎ₁₀＝Ｎ₁ ／Ｎ₂ Ｎ₂₀＝Ｎ₂ ／Ｎ₂ この差分方程式は、論理回路により又はマイクロプロセ
ッサのプログラムで容易に実現できる。

【００６５】同様に、傾斜センサ系の相補フィルタ２８
の伝達関数Ｆ_incl ⁽¹⁾ （ｓ）を双一次変換すると、次の
式が得られる。

【００６６】Ｆ_incl ⁽¹⁾ （ｕ）＝（Ｍ₀ ・ｕ² ＋Ｍ₁ ・
ｕ＋Ｍ₂ ）／（Ｎ₀ ・ｕ² ＋Ｎ₁ ・ｕ＋Ｎ₂ ） ＝Ｙ（ｕ）／Ｘ（ｕ） Ｘ（ｕ）：相補フィルタ２８への入力系列 Ｙ（ｕ）：相補フィルタ２８からの出力系列 Ｍ₀ ＝（−αω_a ｈ＋ｂ₂ ）／Ｋ₂ Ｍ₁ ＝２ｂ₂ ／Ｋ₂ Ｍ₂ ＝（αω_a ｈ＋ｂ₂ ）／Ｋ₂ この式において、Ｘ（ｕ）・ｕⁱ ＝Ｘ_-i、Ｙ（ｕ）・ｕ
ⁱ ＝Ｙ_-iと表すと、出力系列Ｙ（ｕ）は次の差分方程式
で表すことができる。

【００６７】Ｙ（ｕ）＝Ｍ₀₀Ｒ_-2＋Ｍ₁₀Ｒ_-1＋Ｍ₂₀Ｒ
（ｕ）−（Ｎ₀₀Ｒ_-2＋Ｎ₁₀Ｒ_-1＋Ｎ₂₀Ｒ（ｕ）） Ｍ₀₀＝Ｍ₀ ／Ｎ₂ Ｍ₁₀＝Ｍ₁ ／Ｎ₂ Ｍ₂₀＝Ｍ₂ ／Ｎ₂ この差分方程式も、論理回路により又はマイクロプロセ
ッサのプログラムで容易に実現できる。

【００６８】（４．２）オフセット補正レジスタ さらに、この実施例では、ＡＤコンバータ４０の出力か
らオフセット補正レジスタ４６の内容が加算器４４によ
り減じられた値が相補フィルタ２６に入力されている。
オフセット補正レジスタ４６は角速度センサ１０のオフ
セットを記憶するレジスタであり、ＡＤコンバータ４０
の出力から補正レジスタ４６の内容が減ぜられると、オ
フセット補正された値が相補フィルタ２６に入力され、
より誤差を小さくすることができる。

【００６９】また、このオフセット補正レジスタ４６に
は、受信機から受信レベル信号が入力されている。オフ
セット補正レジスタ４６はステップトラック機能を備え
ており、受信レベル信号の値変化に応じてオフセット値
を漸増・漸減させる。オフセット補正レジスタ４６に設
定する初期値としては、常温の値を用いるのが好まし
い。

【００７０】このようにオフセット補正レジスタ４６を
用いる場合、帰還係数Ｋ_b を比較的小さい値に設定す
る。例えば、Ｋ_b ≦５に設定する。ただし、オフセット
補正レジスタ４６の値が初期値から最適値に収束するま
では、帰還係数Ｋ_b を大きく（例えば、Ｋ_b ＞５に）設
定する。

【００７１】従って、この実施例によれば、相補フィル
タ２６及び２８をディジタルフィルタとして実現したた
め、帰還係数Ｋ_b 及び補正係数αを比較的容易に制御で
き、また、オフセット補正レジスタ４６におけるステッ
プトラックによりオフセット誤差を小さくできるばかり
でなく、帰還係数Ｋ_b を比較的小さく設定できるため、
動揺誤差も小さくできる。

【００７２】（５）その他 以上、本発明の各実施例について説明したが、本発明の
構成は以上の実施例のみに限定されるものではない。

【００７３】例えば、図８の制御例では補正係数α及び
帰還係数Ｋ_b を２又は３段階に切り換えていた。この制
御例に示されるように、本発明における適応制御は、必
ずしも学習同定法等による適応型アルゴリズムのみを意
味するものではなく、（α，Ｋ_b ）を２〜３段階で切り
換える程度でも実用になる。

【００７４】また、以上の説明では、補正係数α及び帰
還係数Ｋ_b を適応制御するα−Ｋ_b適応モデル（α−Ｋ
_b adaptation model ）について説明したが、補正係数
αのみを適応的に切り換え又は制御するα適応モデル
（α adaptation model ）でも、動揺誤差の低減を実現
できる。

【００７５】さらに、動揺の周波数及び振幅の検出・パ
ラメータ制御の手法について、ＤＦＴ等の手法やステッ
プトラックの手法を示したが、他の方法、例えば２乗平
均値法、絶対値平均値法やゼロクロス法としても良い。
２乗平均値法は、入力信号（出力θ_o 等）の２乗平均値
を求め、求めた２乗平均値に基づき帰還係数Ｋ_b を決定
する方法であり、絶対値平均値法は、入力信号の絶対値
の平均値を求め、この平均値に基づき帰還係数Ｋ_b を決
定する方法である。ゼロクロス法は、入力信号のゼロク
ロスからその周波数を求め、補正係数αを決定する方法
である。２乗平均、絶対値平均の演算手順や、ゼロクロ
ス検出の方法等は従来周知であるので、ここでは説明を
省略する。

【００７６】

【発明の効果】以上説明したように、本発明によれば、
帰還係数Ｋ_b の設定によりオフセット誤差を低減でき、
補正係数αの導入及びその適応制御により動揺誤差を低
減できる。

【００７７】また、請求項２によれば、補正係数αの適
応制御に加えて、帰還係数Ｋ_b を大きくすることにより
ドリフト誤差を低減できる。

【図面の簡単な説明】

【図１】本発明の第１実施例に係る動揺角検出器の構成
を示すブロック図である。

【図２】この実施例におけるパラメータ適応制御手段の
構成を示すブロック図である。

【図３】伝達関数により第１実施例の構成を示すブロッ
ク図である。

【図４】傾斜センサにおける加速度の影響による誤差
を、補正係数αをパラメータとして示す図であり、特に
帰還係数Ｋ_b ＝５．０の場合の図である。

【図５】傾斜センサにおける加速度の影響による誤差
を、補正係数αをパラメータとして示す図であり、特に
帰還係数Ｋ_b ＝１０．０の場合の図である。

【図６】傾斜センサにおける加速度の影響による誤差
を、補正係数αをパラメータとして示す図であり、特に
帰還係数Ｋ_b ＝１５．０の場合の図である。

【図７】角速度センサのドリフトによる誤差を、帰還係
数Ｋ_b をパラメータとして示す図である。

【図８】パラメータ（α，Ｋ_b ）の制御例を示す図であ
る。

【図９】本発明の第２実施例に係る動揺角検出器の構成
を示すブロック図である。

【図１０】本発明の第３実施例に係る動揺角検出器の構
成を示すブロック図である。

【図１１】本発明の第４実施例に係る動揺角検出器の構
成を示すブロック図である。

【図１２】一従来例に係る動揺角検出器の構成を示すブ
ロック図である。

【図１３】伝達関数により従来例の構成を示すブロック
図である。

【符号の説明】

１０ 角速度センサ １２ 傾斜センサ ２０ 加算器 ２２ パラメータ適応制御手段 ２４ パラメータ適応型相補的合成フィルタ ２６ 帰還ループ付相補フィルタ ２８ 相補フィルタ ３０ 動揺周波数検出手段 ３２ 動揺振幅検出手段 ３４ パラメータ（α，Ｋ_b ）制御手段 θ_i （ｓ） 動揺角検出器の入力（ロール又はピッチ） θ_o （ｓ） 動揺角検出器の出力 Ｇ₁₀（ｓ） 角速度センサの理想的伝達関数 Ｇ₁₁（ｓ） 角速度センサの寄生要素 ｄ₀ 角速度センサのオフセット電圧 ｄ₀ （ｓ） 角速度センサのドリフト Ｈ_b （ｓ） 相補フィルタ２６のドリフト補正用ループ
の伝達関数 ω_a ，ω_b 相補フィルタ２６の遮断周波数 Ｋ_b 相補フィルタ２６のドリフト補正用ループの帰還
係数 Ｇ₂₀（ｓ） 傾斜センサの理想的伝達関数 Ｇ₂₁（ｓ） 傾斜センサの寄生要素 Ｇ₂₂（ｓ） 傾斜センサにおける加速度の影響 Ｆ_incl ⁽¹⁾ （ｓ） 相補フィルタ２８の伝達関数 α 相補フィルタ２８の微分項の補正係数 （α，Ｋ_b ） パラメータ制御信号

Claims (2)

(57)【特許請求の範囲】
1. 【請求項１】 微分特性を有するレートセンサと、 比例特性を有する傾斜センサと、 レートセンサの出力を瀘波する第１の相補フィルタ、傾
   斜センサの出力を瀘波する第２の相補フィルタ及び第１
   の相補フィルタの出力と第２の相補フィルタの出力を加
   算する加算器を含み、レートセンサの出力と傾斜センサ
   の出力とを相補的に合成し所定周波数領域で平坦な周波
   数特性としつつ搭載される移動体の動揺角を出力する相
   補的合成フィルタと、 を備え、 第１の相補フィルタが、帰還係数Ｋ_b の帰還ループを含
   み、 動揺の周波数を検出する手段及び検出結果に応じて補正
   係数α（α≦１）を決定して第２の相補フィルタの伝達
   関数の分子に含まれる微分項をこの補正係数αにより補
   正する制御手段を含むパラメータ適応制御手段を備え、 動揺の周波数に応じ補正係数αの適応制御を行うことを
   特徴とする動揺角検出器。
2. 【請求項２】 請求項１記載の動揺角検出器において、 パラメータ適応制御手段が動揺の振幅を検出する手段を
   備え、 制御手段が検出される動揺の振幅に応じて帰還係数Ｋ_b
   を決定して第１及び第２の相補フィルタに供給し、 動揺の振幅に応じて帰還係数Ｋ_b の適応制御を行うこと
   を特徴とする動揺角検出器。