JP2006145259A - 状態検出装置およびその状態算出方法 - Google Patents

Abstract

【課題】 状態センサの応答速度が遅い場合でも、速い応答速度で状態量を取得することができ、かつ状態速度センサのオフセットがある場合でも発散することなく状態量が取得できる状態検出装置とその状態算出方法を得る。
【解決手段】 本発明の状態検出装置は、状態速度センサ１の情報とオフセット補正値との差分Ａを算出する第１演算器34と、算出差分Ａを積分し積分値を算出する積分器31と、積分値に対し状態センサ１の応答遅れと等しいフィルタ値を算出する遅れフィルタ32と、前記フィルタ値と前記状態センサの情報との差分Ｂを算出する第２演算器35と、前記差分Ｂとゲインとの積を前記オフセット補正値とし、前記差分Ｂを前記積分値から差し引いた値を状態量検出値４とする第３演算器32とを設けたものである。
【選択図】 図１

Description

本発明は、状態検出対象の状態を検出する状態検出装置とその状態算出方法に関する。

従来の状態検出装置の状態算出では、状態センサの情報を状態速度センサなど他のセンサにより補正して使用しているものがある（例えば、特許文献１、特許文献２参照）。
図５は従来の状態検出の処理手順を示すブロック図である。図５において、１０１は状態センサ、１０２は各種センサ、１０３はフィルタ、１０４は補正値演算器、１０５は状態量検出値である。
状態センサ１０１で検出した状態量は、高周波ノイズの除去などの目的でフィルタ１０３にてフィルタ処理される。状態センサ１０１の検出値が、温度、傾斜、振動など状態量以外の要因で変化し、誤差の原因となる場合、これらの誤差要因を検出するための温度センサ、傾斜センサ、加速度センサなどの各種センサ１０２を使用する。補正値演算器３４は、各種センサ１０２により検出された誤差要因が状態センサ１０１の検出値に与える影響を算出し、それを差し引いた値を状態量検出値１０４として使用する。
また、状態速度センサの情報を積分して使用しているものもある（例えば、特許文献３参照）。図６は従来の他の状態検出の処理手順を示すブロック図である。図６において、１１１は状態速度センサ、１１２はフィルタ、１１３は積分器、１１４は状態量検出値である。状態速度センサ１１１で検出した状態量の微分相当値は、高周波ノイズの除去や直流成分除去などの目的でフィルタ１１２にてフィルタ処理される。フィルタ１１２の後段の値を積分器１１３で積分し、状態量検出値１１４として用いる。このように、従来の状態検出装置では、状態センサの情報を他の各種センサ情報により補正して状態量検出値を得る、という手順がとられていた。または、状態速度センサの情報を積分して状態量検出値を得る、という手順がとられていた。
特開平０９−０８１３０８号公報 特願平０５−１４１９７０号公報 特開平０５−００１９２０号公報

ところが、従来の状態検出装置の状態算出方法では、状態センサの情報を他の各種センサ情報により補正して状態量検出値を得るという手順をとっているので、状態センサに頼る部分が大きくなっており、状態センサの応答速度が遅い場合には、他の各種センサを用いて補正しても元の状態センサの遅れ分が除去できないため、実際の状態量との誤差が大きいという問題があった。また、状態量の検出に状態速度センサの積分値を用いるような場合は、状態速度センサのオフセット分の誤差が積算され、時間が経つにつれて状態量検出値が増大して発散するという問題もあった。この問題に対し、別に状態センサを用い、状態量が０となったときの状態速度センサのオフセット分を記録し、状態速度センサ出力値から差し引くなどの処理を行いオフセット分の誤差を減らす方法もあるが、完全に誤差をなくすことはできないため、問題を小さくしたにすぎず、長時間積分を続ける場合にはやはり状態量検出値が発散するという問題があった。
本発明はこのような問題点に鑑みてなされたものであり、状態センサの応答速度が遅い場合に、応答速度の速い状態速度センサ情報を用いることにより速い応答速度で状態量を取得することができるようにするとともに、状態速度センサのオフセットがある場合でも発散することなく状態量を取得することができる方法を提供することを目的とする。

上記問題を解決するため、本発明は、次のようにしたものである。
請求項１に記載の発明は、状態検出対象の状態量を検出する状態センサと、前記状態量の微分相当値を検出する状態速度センサと、前記状態センサおよび前記状態速度センサから得た情報を用いて状態量検出値の算出を行う状態検出演算部とを備えた状態検出装置の状態算出方法において、前記状態速度センサの情報とオフセット補正値との差分Ａを積分した積分値を算出し、前記積分値に対し前記状態センサの応答遅れと等しい遅れフィルタ処理をしたフィルタ値を算出し、前記フィルタ値と前記状態センサの情報との差分Ｂを算出し、前記差分Ｂとゲインとの積を前記オフセット補正値とし、前記差分Ｂを前記積分値から差し引いた値を状態量検出値とする手順で処理するものである。
請求項２に記載の発明は、状態検出対象の状態量を検出する状態センサと、前記状態量の微分相当値を検出する状態速度センサと、前記状態センサおよび前記状態速度センサから得た情報を用いて状態量検出値の算出を行う状態検出演算部とを備えた状態検出装置において、状態速度センサの情報とオフセット補正値との差分Ａを算出する第１演算器と、前記算出差分Ａを積分し積分値を算出する積分器と、前記積分値に対し前記状態センサの応答遅れと等しいフィルタ値を算出する遅れフィルタと、前記フィルタ値と前記状態センサの情報との差分Ｂを算出する第２演算器と、前記差分Ｂとゲインとの積を前記オフセット補正値とし、前記差分Ｂを前記積分値から差し引いた値を状態量検出値とする第３演算器とを設けたものである。
請求項３に記載の発明は、前記状態を位置、前記状態速度を速度としたものである。
請求項４に記載の発明は、前記状態を速度、前記状態速度を加速度としたものである。
請求項５に記載の発明は、前記状態を角度、前記状態速度を角速度としたものである。
請求項６に記載の発明は、前記状態センサに傾斜センサ、前記状態速度センサにジャイロセンサを用い、前記状態量を制御対象の傾斜角度、前記状態速度を制御対象の傾斜角速度としたものである。

請求項１、２に記載の発明によると、状態センサの応答速度が遅い場合に、応答速度の速い状態速度センサの情報を用いることにより、速い応答速度で状態量を取得することができるようにするとともに、状態速度センサのオフセットがある場合でも発散することなく状態量を取得することができる。
請求項３に記載の発明によると、位置センサの応答速度が遅い場合に、応答速度の速い速度センサ情報を用いることにより、速い応答速度で位置情報を取得することができるようにするとともに、速度センサのオフセットがある場合でも発散することなく位置情報を取得することができる。
請求項４に記載の発明によると、速度センサの応答速度が遅い場合に、応答速度の速い加速度センサ情報を用いることにより、速い応答速度で速度情報を取得することができるようにするとともに、加速度センサのオフセットがある場合でも発散することなく速度情報を取得することができる。
請求項５に記載の発明によると、角度センサの応答速度が遅い場合に、応答速度の速い角速度センサ情報を用いることにより、速い応答速度で角度情報を取得することができるようにするとともに、角速度センサのオフセットがある場合でも発散することなく角度情報を取得することができる。
請求項６に記載の発明によると、傾斜センサの応答速度が遅い場合に、応答速度の速いジャイロセンサ情報を用いることにより、速い応答速度で傾斜情報を取得することができるようにするとともに、ジャイロセンサのオフセットがある場合でも発散することなく傾斜情報を取得することができ、得られた応答の速い傾斜情報を用いることにより制御対象である倒立振子の直立を維持するための安定性を高めることができる。

以下、本発明の具体的実施例を、図に基づいて説明する。

本発明の実施例１を図１に示す。図１は状態検出の処理手順のブロック図である。図において、１は状態センサ、２は状態速度センサ、３は状態検出演算部、４は状態量検出値である。状態検出演算部３は、積分器３１、遅れフィルタ３２、ゲイン３３、第１演算器３４、第２演算器３５および第３演算器３６からなる。
状態量は状態センサ１の出力として検出される。ただし状態センサ１の応答速度が遅い場合には、そのままフィードバックしても要求される応答速度で制御対象の状態量を制御できない場合がある。本発明では、状態センサ１の応答遅れと等しい遅れフィルタ３２と、応答速度が十分に速い状態速度センサ２とを用いることにより、この問題を解決する。状態速度センサ２は状態量の微分相当値を検出するためのものである。状態速度センサ２の出力を時間積分することにより状態量が得られると考えられるが、状態速度センサ２の出力に誤差として直流成分が含まれる場合、その積分値は直流成分が積算され、発散する。本発明はこの問題も解決する。
状態センサ１の出力情報Ｐ、状態速度センサ２の出力情報Ｖ、積分器３１の出力をＩ、遅れフィルタ３２の出力をＦとし、ゲイン３３のゲインをＧとする。状態センサ１の応答遅れは時定数Ｔの１次遅れで表されるものとする。状態量検出値Ｐｆｂを以下のように算出する。ｓはラプラス演算子である。
Ｆ＝Ｉ／（Ｔｓ＋１）
Ｉ＝（Ｖ−Ｇ（Ｆ−Ｐ））／ｓ
Ｐｆｂ＝Ｉ−（Ｆ−Ｐ）
Ｆは状態速度センサ２の出力を積分し、状態センサ１の応答遅れと等しい遅れフィルタ処理をしたものである。この値は、状態センサ１の出力Ｐを状態速度センサ２の出力情報を用いて推定したものと見ることができる。状態センサ１の出力は応答遅れがある以外は制御対象の状態量を正しく表していると考えられるため、（Ｆ−Ｐ）を積分後の値Ｉから差し引くことによって正しい状態量を示す状態センサ１の出力値に近づき、誤差を減らす効果がある。また、状態速度センサの出力ＶからＧ（Ｆ−Ｐ）を差し引いて積分することにより、積分値の発散を防ぐことができる。ゲインＧは大きくするほど状態量検出値６のオフセット分の誤差を減らす効果があり、例えばＧ＝１としてもよい。
ＣＰＵを用いて離散演算を行う場合は例えば以下のように離散化して算出すればよい。tは演算の周期を表す。
Ｆ（ｋ）＝（ｔＩ（ｋ−１）＋（Ｔ−ｔ）Ｆ（ｋ−１））／Ｔ
Ｉ（ｋ）＝Ｉ（ｋ−１）＋ｔ（Ｖ（ｋ）−Ｇ（Ｆ（ｋ）−Ｐ（ｋ））） （ｋ：１, ２,・・）
Ｐｆｂ（ｋ）＝Ｉ（ｋ）−（Ｆ（ｋ）−Ｐ（ｋ））

本発明の実施例２を図２に示す。図２は本発明の方法を制御に適用した制御装置を示すブロック図である。図において、５は制御対象、６は制御器である。他の符号は実施例１と同じであるため説明を省略する。
本発明の状態検出方法は、状態制御装置において、制御したい制御対象５の状態量をフィードバック情報として取得するために使用できる。制御対象５への指令は制御器６により与えられる。制御器６は状態量と状態速度をフィードバック情報として用いて指令を生成する。制御器６は、従来の制御理論を用いて設計することができ、例えば極配置法、最適制御、適応制御、Ｈ∞制御などの設計手法を用いればよい。制御器で用いる状態量の検出値は、本発明の方法を用いて算出する。制御システムに対して本発明の方法を用いることにより、応答速度の速い状態量検出値が得られるため、制御系の安定性が向上し、制御ゲインを高めることができ、制御性能が向上する。

本発明の実施例３を図３に示す。図３は倒立振子制御装置に用いた構成例を示すブロック図である。図において、７は倒立振子、８は傾斜センサ、９はジャイロセンサである。制御対象の倒立振子７は、人が立位で乗車して直立姿勢を保つ制御を行いながら走行する車両や、倒立振子形の移動機構を持つロボットの場合でも同様である。図で、状態量は倒立振子７の傾斜角度、状態速度は傾斜角速度であり、それぞれ傾斜センサ８、ジャイロセンサ９により検出される。傾斜センサ８に例えば液面レベルの変化により傾斜を検出する静電容量形傾斜センサなどを用いた場合、応答時間は数百msから数秒程度と遅く、そのまま用いると倒立振子７は倒立状態を維持することが困難である。ところがジャイロセンサ９は応答が高速なものが多く開発されており、本発明の方法が適用できる。ゲイン１０、ゲイン１１は例えば極配置法や最適制御により決定するか、あるいは実験やシミュレーションにより試行錯誤的に決定すればよい。

本発明の実施例４を図４に示す。図４は本発明の方法をナビゲーション装置に適用した構成を示すブロック図である。図において、１２はセンサ部、１３ 演算部、１４はナビゲーション処理部、１５は画面である。センサ部１２は、ＧＰＳレシーバ１２ａ、車速パルスセンサ１２ｂ、加速度センサ１２ｃ、地磁気センサ１２ｄおよびジャイロセンサ１２ｅからなる。ＧＰＳレシーバ１２ａは車両の位置を受信し、車速パルスセンサ１２ｂは走行中の車両の速度を車輪の回転数から算出し、加速度センサ１２ｃは車両の加速度を検出し、地磁気センサ１２ｄは車両の進行方向を検出し、ジャイロセンサ１２ｅは鉛直方向軸回りの車両の回転角速度を検出する。また、演算部１３は、位置演算部１３ａ、速度演算部１３ｂおよび方位演算部１３ｃからなる。
位置演算部１３ａは、ＧＰＳレシーバ１２ａおよび車速パルスセンサ１２ｂの出力情報を用いて車両の現在位置を算出する。速度演算部１３ｂは、車速パルスセンサ１２ｂおよび加速度センサ１２ｃの出力情報を用いて車両の走行速度を算出する。方位演算部１３ｃは地磁気センサ１２ｄおよびジャイロセンサ１２ｅの出力情報を用いて車両の進行方向を算出する。画面１５は地図上に車両の現在位置、進行方向、目的地までのルートなどを表示するためのもので、ナビゲーション処理部１４は位置・速度・方位の各演算部より受け取った現在位置、速度、進行方向から目的地までのルートなどを算出して画面１５に表示するための画像を生成するものである。位置・速度・方位の各演算部に本発明の方法を用いる。状態センサとしてＧＰＳレシーバ１２ａ、状態速度センサとして車速パルスセンサ１２ｂを用い、遅れフィルタをＧＰＳレシーバ１２ａより位置情報が得られる周期に等しい零次ホールドとして本発明の方法を適用することにより、ＧＰＳレシーバ１２ａによる位置情報が遅い周期でしか得られない場合でも速い応答速度で車両の位置を算出できる。また、状態センサとして車速パルスセンサ１２ｂ、状態速度センサとして加速度センサ１２ｃを用い、遅れフィルタを車速パルスセンサより車速情報が得られる周期に等しい零次ホールドとして本発明の方法を適用することにより、車速パルスセンサ１２ｃによる速度情報が遅い周期でしか得られない場合でも速い応答速度で車速を算出できる。また、状態センサとして地磁気センサ１２ｄ、状態速度センサとしてジャイロセンサ１２ｅを用い、遅れフィルタを地磁気センサ１２ｄの応答速度に等しい一次遅れフィルタとして本発明の方法を適用することにより、地磁気センサ１２ｄによる方位情報の応答が遅い場合でも速い応答速度で進行方向を算出できる。

このように、状態速度センサ情報とオフセット補正値との差分Ａを積分した積分値を算出し、前記積分値に対し前記状態センサの応答遅れと等しい遅れフィルタ処理をしたフィルタ値を算出し、前記フィルタ値と前記状態センサ情報の差分Ｂを算出し、前記差分Ｂとゲインとの積を前記オフセット補正値とし、前記差分Ｂを前記積分値から差し引いた値を状態情報とする、という手順をとるので、状態センサの応答速度が遅い場合に、応答速度の速い状態速度センサ情報を用いることにより、速い応答速度で状態量を取得することができるようにするとともに、状態速度センサのオフセットがある場合でも発散することなく状態量を取得することができる。

状態速度センサ情報とオフセット補正値との差分Ａを積分した積分値を算出し、前記積分値に対し前記状態センサの応答遅れと等しい遅れフィルタ処理をしたフィルタ値を算出し、前記フィルタ値と前記状態センサ情報の差分Ｂを算出し、前記差分Ｂとゲインとの積を前記オフセット補正値とし、前記差分Ｂを前記積分値から差し引いた値を状態情報とする、という手順をとるため、状態センサの応答速度が遅い場合に、応答速度の速い状態速度センサ情報を用いることにより、速い応答速度で状態量を取得することができるようにするとともに、状態速度センサのオフセットがある場合でも発散することなく状態量を取得することができて、速い応答速度が要求される制御、ナビゲーションシステムという用途にも適用できる。

本発明の実施例１を示す状態検出手順のブロック図 本発明の実施例２を示す制御装置適用例のブロック図 本発明の実施例３を示す倒立振子制御装置適用例のブロック図 本発明の実施例４を示すナビゲーション装置適用例のブロック図 従来の方法を示す状態検出手順のブロック図 従来の他の方法を示す状態検出手順のブロック図

符号の説明

１、１０１ 状態センサ
２、１１１ 状態速度センサ
３ 状態検出演算部
３１ 積分器
３２ 遅れフィルタ
３３ ゲイン
３４ 第１演算器
３５ 第２演算器
３６ 第３演算器
４、１０５、１１４ 状態量検出値
５ 制御対象
６ 制御器
７ 倒立振子
８ 傾斜センサ
９ ジャイロセンサ
１０、１１ ゲイン
１２ センサ部
１２ａ ＧＰＳレシーバ
１２ｂ 車速パルスセンサ
１２ｃ 加速度センサ
１２ｄ 地磁気センサ
１２ｅ ジャイロセンサ
１３ 演算部
１３ａ 位置演算部
１３ｂ 速度演算部
１３ｃ 方位演算部
１４ ナビゲーション処理部
１５ 画面
１０２ 各種センサ
１０３、１１２ フィルタ
１０４ 補正値演算器

Claims (6)

1. 状態検出対象の状態量を検出する状態センサと、前記状態量の微分相当値を検出する状態速度センサと、前記状態センサおよび前記状態速度センサから得た情報を用いて状態量検出値の算出を行う状態検出演算部とを備えた状態検出装置の状態算出方法において、
   前記状態速度センサの情報とオフセット補正値との差分Ａを積分した積分値を算出し、
   前記積分値に対し前記状態センサの応答遅れと等しい遅れフィルタ処理をしたフィルタ値を算出し、
   前記フィルタ値と前記状態センサの情報との差分Ｂを算出し、
   前記差分Ｂとゲインとの積を前記オフセット補正値とし、
   前記差分Ｂを前記積分値から差し引いた値を状態量検出値とする手順で処理することを特徴とする状態検出装置の状態算出方法。
2. 状態検出対象の状態量を検出する状態センサと、前記状態量の微分相当値を検出する状態速度センサと、前記状態センサおよび前記状態速度センサから得た情報を用いて状態量検出値の算出を行う状態検出演算部とを備えた状態検出装置において、
   前記状態速度センサの情報とオフセット補正値との差分Ａを算出する第１演算器と、前記算出差分Ａを積分し積分値を算出する積分器と、前記積分値に対し前記状態センサの応答遅れと等しいフィルタ値を算出する遅れフィルタと、
   前記フィルタ値と前記状態センサの情報との差分Ｂを算出する第２演算器と、
   前記差分Ｂとゲインとの積を前記オフセット補正値とし、前記差分Ｂを前記積分値から差し引いた値を状態量検出値とする第３演算器とを設けたことを特徴とする状態検出装置。
3. 前記状態量は位置、前記状態速度は速度であることを特徴とする請求項２記載の状態検出装置。
4. 前記状態量は速度、前記状態速度は加速度であることを特徴とする請求項２記載の状態検出装置。
5. 前記状態量は角度、前記状態速度は角速度であることを特徴とする請求項２記載の状態検出装置。
6. 前記状態センサは傾斜センサ、前記状態速度センサはジャイロセンサであり、前記状態量は制御対象の傾斜角度、前記状態速度は制御対象の傾斜角速度であることを特徴とする請求項２または５記載の状態検出装置。

Images