JP4281777B2 - 傾斜角推定機構を有する移動体 - Google Patents

Description

本発明は、傾斜角推定機構を有する移動体に関し、特に傾斜角推定機構を有する倒立２輪型車両に関する。

倒立２輪型車両のような移動体においては、倒立姿勢を維持するために傾斜角度を高精度に推定する必要がある。これは、２輪型車両に限らず、歩行型移動体やキャタピラ駆動の移動体においても、傾斜角度を高精度に推定することが求められている。

従来、傾斜角を推定する機構としては、ジャイロ（角速度）センサを用いる方式や加速度センサを用いる方式がある。ジャイロセンサを用いる方式においては、ジャイロセンサの出力信号を積分することによって傾斜角度を求めることができるが、ジャイロセンサの出力信号には本来的にオフセットノイズが重畳されているため、真値に対して少しずつずれが大きくなっていくという問題があった。

加速度センサを用いる方式では、２軸以上の加速度センサによる幾何学的関係を用いて傾斜角度を算出する。この方式によれば、２軸重力加速度の値から傾斜角度を算出しているため、静的な状態でなければ推定精度が悪化するという問題があった。特に、移動体が加減速した場合には、加減速によって発生する成分が２軸の加速度センサにノイズとして重畳することになる。

また、ジャイロセンサと３軸の加速度センサを用いて自律慣性航法（ＩＮＳ）により傾斜角度を推定する方式も提案されている。しかしながら、上述したようなジャイロセンサにおけるオフセットノイズの問題や加速度センサにおける加減速に基づくノイズの問題は解消されない。

なお、ジャイロセンサ及び加速度計を周波数帯域ごとに切り替えて用いる技術が特許文献１に開示されているが、これは傾斜角推定機構を有する移動体に関する技術ではない。
特開平４−３４６０２１号公報

以上説明したように、ジャイロセンサや加速度センサを用いることにより車体傾斜角度を求めることは可能であるが、精度に問題があった。

本発明は、かかる課題を解決するためになされたものであり、高精度に車体傾斜角度を推定することが可能な傾斜角推定機構を有する移動体を提供することを目的とする。

本発明にかかる移動体は、２軸方向の加速度を検出する加速度センサと、車体の角速度を検出するジャイロセンサと、車体の傾斜角を推定する傾斜角推定機構とを備えた移動体であって、当該傾斜角推定機構は、前記加速度センサの検出した加速度に基づいて推定された傾斜角と、前記ジャイロセンサの検出した角速度に基づいて、移動体の線形モデル式をオブザーバとして、車体の傾斜角を推定する手段を有し、車体の傾斜角を推定する場合に、オブザーバゲインの値を変化させて前記加速度センサによる傾斜角或いは前記ジャイロセンサによる傾斜角に近づけるものである。

ここで、前記傾斜角推定機構は、オブザーバゲインＫを任意の値に設定する設定手段を有することが望ましい。

また、前記設定手段は、移動体が前後加速状態にある場合に、前記ジャイロセンサによる傾斜角に近づけるように、前記オブザーバゲインＫの値を設定するとよい。

さらに、前記設定手段は、加速度センサ及びジャイロセンサの故障状態に応じて、当該オブザーバゲインＫを設定することが望ましい。
また、前記加速度センサの後段にローパスフィルタを設けるとよい。
好適な実施の形態における移動体は、倒立２輪型車両である。

本発明によれば、高精度に車体傾斜角度を推定することが可能な傾斜角推定機構を有する移動体を提供することができる。

図１に、本発明にかかる傾斜角推定機構を有する移動体のセンサ座標系を示す。図１に示されるように、当該移動体１０は、車体１と、車体１に対して図示しない駆動機構を介して固定された車輪２を有している。

当該移動体１０の前方（即ち、移動方向、ピッチ方向）に対して直交し、ほぼ水平方向（即ち、ロール方向）に延びる軸をＸ軸、当該移動体１０の前方（即ち、移動方向、ピッチ方向）をＹ軸、鉛直方向をＺ軸としている。本例では、車体１と車輪２の回転中心を結ぶ線と、Ｚ軸のなす角度を車体傾斜角とする。

移動体１０は、Ｙ軸方向及びＺ軸方向の加速度を計測する加速度センサとその後段にカットオフ周波数の低いローパスフィルタ（低域通過フィルタ）を備えている。これら２つの加速度センサによって検出されたＹ軸方向及びＺ軸方向の加速度u(1),u(2)に基づき次式に従って車体傾斜角（即ち、ピッチ角）ηを求めることができる。
η＝tan^−１（u(1)／u(2)） ・・・（１）

また、当該移動体１０は、Ｘ軸方向の角速度を計測する１軸のジャイロセンサ（角速度センサ）を備えている。さらに、移動体１０では、加速度センサ及びジャイロセンサの検出信号や制御信号を入力し、車体傾斜角を推定（算出）する傾斜角推定機構をコンピュータにより実現している。

この傾斜角推定機構は、加速度センサの検出した加速度に基づいて傾斜角を推定する手段と、ジャイロセンサの検出した角速度に基づいて傾斜角を推定する手段と、加速度センサに基づく傾斜角と、ジャイロセンサに基づく傾斜角に基づいて、移動体の線形モデル式をオブザーバとして傾斜角を推定する手段とを備えている。このとき、傾斜角を推定する手段は、ジャイロセンサに基づく傾斜角をオブザーバの参照値とし、ジャイロセンサに基づく傾斜角に対して推定値がフィードバック追従するようにして推定する。

図２は、本発明にかかる傾斜角推定処理を説明するためのブロック図である。図に示されるように、制御信号がＩ_１に、Ｙ軸加速度センサにより検出されたＹ軸方向の加速度信号がＩ_２に、Ｚ軸加速度センサにより検出されたＺ軸方向の加速度信号がＩ_３に、１軸ジャイロセンサにより検出された角速度信号がＩ_４に対してそれぞれ入力される。

まず、Ｉ_２に入力されたＹ軸方向の加速度と、Ｉ_３にＺ軸方向の加速度に基づいて、上記式（１）に従って、幾何学的に車体傾斜角が算出される。ここで算出された傾斜角は、直流（ＤＣ）成分〜低周波域に対して高精度であるが、加減速を伴う運動時には加速度センサに外乱が印加されるので、中周波数域〜高周波数域に対して低精度である。

次に、制御信号、算出された車体傾斜角、ジャイロセンサにより検出された角速度を入力し、さらに外力オブザーバによるフィードバック処理によって、最終的に傾斜角、傾斜角速度及び外力を算出し、それぞれＯ_１，Ｏ_２，Ｏ_３より出力している。なお、図２において、外力オブザーバに関する式ｙ（ｎ）＝Ｃｘ（ｎ）＋Ｄｕ（ｎ）を式（２）とし、ｘ（ｎ＋１）＝Ａｘ（ｎ）＋Ｂｕ（ｎ）を式（３）とする。

次に、外力オブザーバ設計の詳細について説明する。
まず、車体傾斜角の線型モデル化を実行する。倒立二輪車の車体モデル（運動方程式）は次式の通り表わすことができる。
ここで、η：車体の傾斜角、θ：車輪の回転角、ｍ_１：車体の質量、ｍ_２：車輪の質量、ｌ：車体重心の車軸からの距離、Ｊ_１：車体の重心回りの慣性モーメント、Ｊｍ：モータロータの慣性モーメント、Ｊｗ：車輪の軸回りの慣性モーメント、ｎ：ギア比（モータ：車輪＝１：ｎ）、ｒ：車輪半径、ｆｒ：駆動システムの抵抗、ｕ：モータのトルク値である。

上記式（４）,（５）のうち、本発明においては車体傾斜角ηの推定を実行できればよいため、式（４）のみを考慮する。なお、式（５）は、傾斜角速度を求める場合に利用される。式（４）の左辺をηでまとめると、次の式（６）の通りとなる。

この式（６）を基本モデルとして外力オブザーバを作成する。ここで、式（６）の右辺のθ項は、η方程式の外力として働く。本発明では、当該θ項を無視すると、推定誤差が悪化するため、これを防止するために、状態量として外力ｆも考慮した外力オブザーバを構成した。

次に、外力オブザーバの基本モデルの導出について説明する。今、状態量を次式（７）とおく。
ここで、ｆは外力である。

式７で示される状態量を用いて、式６を状態方程式表現に変換すると、次の式（８）,（９）のように表わすことができる。
ここで、外力ｆについては、サンプル周期の間は一定値をとるステップ状とし、その間の微分値は０とした。

次に外力オブザーバのゲインの決定方法について説明する。
外力オブザーバは、次式（１０）により表わすことができる。

式（１０）におけるオブザーバゲイン（フィードバックゲイン）Ｋは、各状態量η、ηドット、ｆ毎にＫ１、Ｋ２、Ｋ３のようにそれぞれ設定される。式（１０）におけるオブザーバゲインＫの大きさによって、推定の収束速度が決定される。つまり、式（１０）から求められる特性方程式の極の大きさによって、推定の収束速度が決まる。
Ｋ＝（Ｋ１，Ｋ２，Ｋ３）としたとき、Ｋ１が大きれば加速度センサによる推定値、Ｋ２が大きければジャイロセンサによる推定値に各々近づく。低周波では加速度センサの推定値が正しいと判断されるため、Ｋ１を大きくし、中〜高周波ではＫ２を大きくする。動作周波数が予め推定できる場合は、その周波数に応じてＫを設定すればよい。

具体的には、次の（Ａ）（Ｂ）間のトレードオフに留意してＫを決定する。
（Ａ）オブザーバゲインＫが大きい（極が実軸から遠い）場合
この場合には、加速度センサ入力である観測量の影響を大きく受ける。従って、推定値は観測量を優先することになり、基本モデルの影響は小さくなる。このため、センサノイズの影響を受けやすくなる。また、この場合には、推定の収束速度は高くなる。

（Ｂ）オブザーバゲインＫが小さい（極が実軸に近い）場合
この場合には、加速度センサ入力である観測量を大きくなますことになる。換言すると、観測量の影響を小さくすることになる。従って、基本的に推定値は基本モデルに従うことになる。このため、センサノイズの影響は受けにくくなる。また、この場合には、推定の収束速度は低くなる。

オブザーバゲインＫは、ジャイロセンサに基づき推定された傾斜角の高周波成分はそのまま出力し、低域及び直流成分は加速度センサに基づき推定された傾斜角となるようにすることで、直流成分から高周波域までの広範な周波数域で高精度に傾斜角を推定できる。

本発明において用いられる外力オブザーバは、傾斜角の直流成分を推定するのが目的であるため、その応答は遅くてもよい。応答が遅いということは、それだけノイズを低減できることを示すので、より精度の高い推定が期待される。また、応答の遅いオブザーバとするため、極は比較的実軸の近くに設定する。

本発明においては、上述のように、外力オブザーバには、傾斜角に対応する運動方程式（モデル情報）が利用されている。このモデル情報と１軸ジャイロセンサにより検出した角速度に基づき、中周波域〜高周波域において高精度に傾斜角を推定している。さらに、本発明では、１軸ジャイロセンサのみならず、２軸加速度センサに基づき推定された傾斜角をオブザーバの参照値として、これに推定値が追従するようにフィードバック制御している。これによって、直流成分から高周波域までの広範な周波数域で高精度に傾斜角を推定できる。また、加速度センサのノイズについては、オブザーバゲインＫを適切に設定することによって、そのノイズの影響を低減することができる。

最後に設計した連続系外力オブザーバを離散化することによって外力オブザーバを設計することができる。

さらに、本発明では、オブザーバゲインＫの制御を制御機構により実行している。図３に当該制御のフローチャートを示す。まず、制御機構は、ジャイロセンサ等によって検出した角速度が予め定められた所定範囲内であるかを判定する（Ｓ１０１）。ここで、角速度を基準としたのは、移動体が前後加速状態にあるかどうかを検出することができるからである。したがって、図３の示す例では、角速度に基づき移動体が前後加速状態にあるかどうかを検出したが、これに限らず、他のパラメータに基づき、当該前後加速状態にあるかを検出するようにしてもよい。制御機構は、角速度が所定範囲内であると判定した場合には、オブザーバゲインＫの値を変更する制御は実行しない。

次に制御機構が、角速度が所定範囲内にない、即ち所定範囲外であると判定した場合には、さらに、角速度が所定範囲よりも高いかどうかを判定する（Ｓ１０２）。判定の結果、制御機構が、角速度が所定範囲よりも高いと判定した場合には、オブザーバゲインＫの値を小さくする制御を実行する（Ｓ１０３）。これにより、前後加速状態にあり加速度センサに外乱ノイズが発生しやすい状態にあるため、加速度センサの影響を低減することができる。

判定の結果、制御機構が、角速度が所定範囲よりも高くない、即ち低いと判定した場合には、オブザーバゲインＫの値を大きくする制御を実行する（Ｓ１０４）。なお、オブザーバゲインＫの値の大きさの制御については、オブザーバゲインＫを小さくして推定処理した場合と、オブザーバゲインＫを大きくして推定処理した場合のそれぞれの出力値のいずれかを選択するようにしてもよい。

続いて、センサの故障制御について説明する。本発明では、ジャイロセンサと加速度センサを備えているが、いずれかのセンサが故障した場合には、故障したセンサに基づく傾斜角推定は実行しないように、即ち、故障していないセンサのみに基づいて傾斜角推定を実行するように制御する。

具体的には、制御機構は、ウォッチドッグ等によって、断線やセンサ故障等の故障を検出したかどうかを判定する（Ｓ２０１）。制御機構は、故障を検出したものと判定した場合に、ジャイロセンサの故障かどうかを判定する（Ｓ２０２）。制御機構は、ジャイロセンサの故障であると判定した場合には、故障していない加速度センサにより車体傾斜角を推定する（Ｓ２０３）。他方で、制御機構は、ジャイロセンサの故障でないと判定した場合には、加速度センサの故障かどうかを判定する（Ｓ２０４）。判定の結果、制御機構が加速度センサの故障と判定した場合には、故障していないジャイロセンサにより車体傾斜角を推定する（Ｓ２０５）。制御機構が、加速度センサの故障でないと判定した場合には、推定処理を停止する（Ｓ２０６）。

さらに、本発明においては、移動体のモデル情報を用いて、傾斜角の推定精度を高めることができる。例えば、車両の構造上の制約から、傾斜角が所定値以上にならない場合には、推定値がその所定値を超える場合に、その所定値に制限した上で推定処理を実行することにより真値と推定値の偏差が限られるため、推定精度の悪化を防止することが可能となる。

なお、上述の例では、移動体１０は、倒立２輪型の不安定車両であったが、これに限らず、歩行型移動体やキャタピラ駆動の移動体であってもよい。また、移動体１０は、搭乗型の移動体であってもよく、さらには、ロボットの下半身に組み込まれた移動体や、荷物を搭載して移動する移動体であっても適用可能である。

本発明の実施の形態にかかる移動体のセンサ座標系を説明するための図である。 本発明の実施の形態にかかる移動体の傾斜角推定法を示すブロック図である。 本発明の実施の形態にかかる移動体の傾斜角推定方法を示すフローチャートである。 本発明の実施の形態にかかる移動体の傾斜角推定方法を示すフローチャートである。

符号の説明

１ 車体
２ 車輪
１０ 移動体

Claims (6)

1. ２軸方向の加速度を検出する加速度センサと、
   車体の角速度を検出するジャイロセンサと、
   車体の傾斜角を推定する傾斜角推定機構とを備えた移動体であって、
   当該傾斜角推定機構は、
   前記加速度センサの検出した加速度に基づいて推定された傾斜角と、前記ジャイロセンサの検出した角速度に基づいて、移動体の線形モデル式をオブザーバとして、車体の傾斜角を推定する手段を有し、
   車体の傾斜角を推定する場合に、オブザーバゲインＫの値を変化させて前記加速度センサによる傾斜角或いは前記ジャイロセンサによる傾斜角に近づける移動体。
2. 前記傾斜角推定機構は、オブザーバゲインＫを任意の値に設定する設定手段を有することを特徴とする請求項１記載の移動体。
3. 前記設定手段は、移動体が前後加速状態にある場合に、前記ジャイロセンサによる傾斜角に近づけるように、前記オブザーバゲインＫの値を設定することを特徴とする請求項２記載の移動体。
4. 前記設定手段は、加速度センサ及びジャイロセンサの故障状態に応じて、当該オブザーバゲインＫを設定することを特徴とする請求項２記載の移動体。
5. 前記加速度センサの後段にローパスフィルタを設けたことを特徴とする請求項１〜４いずれか１項に記載の移動体。
6. 前記移動体は、倒立２輪型車両であることを特徴とする請求項１〜５いずれか１項に記載の移動体。