JP5880726B2 - 倒立型移動体及びその制御方法 - Google Patents

Description

本発明は、倒立型移動体及びその制御方法に関し、特に、センサが検出した角速度に基づいて倒立型移動体を倒立制御する技術に関する。

倒立二輪車等の倒立型移動体において、システムに異常が発生した場合でも安全に搭乗者が降車できるようにすることは、重要な事項の１つである。倒立型移動体は、センサからの出力に基づいて倒立制御を行っている。そのため、センサの故障及び故障したセンサを精度良く検出し、故障したセンサからの出力に基づいた倒立制御を抑止することが、安全性を担保する上で非常に重要となってくる。

特許文献１には、電源とセンサーエレクトロニクスボードと制御プロセッサの組み合わせが一まとめにされたパワーベースが三重余剰で構成された車両が開示されている。この車両は、三重の余剰センサのそれぞれから供給されるデータを比較することで、センサの故障を検出するようにしている。

しかしながら、特許文献１に開示の車両のように、単純に同一構成でセンサを複数余剰としてしまうと、コストが増大してしまうという問題がある。例えば直交三軸の角速度を検出するために、センサーエレクトロニクスボードにおいて３つの直交三軸のそれぞれに対してジャイロセンサを搭載するようにした場合には、センサーエレクトロニクスボードが三重余剰であるため、合計で９つ（３×３）もジャイロセンサが必要となってしまう。すなわち、コストが増大してしまうという問題がある。なお、特許文献２には、相互に斜交配置された４つのジャイロで計測した角速度から、直交三軸の角速度を算出する慣性基準装置が開示されている。

特表２００３−５１７３４０号公報 特開２００９−２０４４１９号公報

本発明は、上述した知見に基づいてなされたものであって、故障検出精度を低下させることなく、コストを低減することができる倒立型移動体及びその制御方法を提供することを目的とする。

本発明の第１の態様にかかる倒立型移動体は、倒立制御される倒立型移動体であって、前記倒立型移動体のヨー軸に直交する平面上で前記倒立型移動体のピッチ軸から第１の所定角度傾斜させた軸周りの角速度を検出する第１のセンサと、前記倒立型移動体のヨー軸に直交する平面上で前記倒立型移動体のピッチ軸から第２の所定角度傾斜させた軸周りの角速度を検出する第２のセンサと、前記倒立型移動体のピッチ軸周りの角速度を検出する第３のセンサと、前記倒立型移動体のピッチ軸の加速度を検出するピッチ軸加速度検出部と、前記倒立型移動体のロール軸の加速度を検出するロール軸加速度検出部と、前記倒立型移動体のヨー軸の加速度を検出するヨー軸加速度検出部と、前記第１のセンサ、前記第２のセンサ、及び前記第３のセンサのそれぞれが検出した角速度に基づいて前記倒立制御を行う制御部と、を備え、前記制御部は、前記第１のセンサ及び前記第２のセンサのそれぞれで検出した角速度に基づいて算出した前記倒立型移動体のピッチ軸周りの第１角速度と、前記第３のセンサが検出した前記倒立型移動体のピッチ軸周りの第２角速度と、前記ピッチ軸加速度検出部、前記ロール軸加速度検出部、及び前記ヨー軸加速度検出部のそれぞれで検出した加速度に基づいて算出した前記倒立型移動体のピッチ軸周りの第３角速度の相互関係に基づいて、特定の安全機能を発動するものである。

本発明の第２の態様にかかる制御方法は、倒立型移動体のヨー軸に直交する平面上で前記倒立型移動体のピッチ軸から第１の所定角度傾斜させた軸周りの角速度を検出する第１のセンサと、前記倒立型移動体のヨー軸に直交する平面上で前記倒立型移動体のピッチ軸から第２の所定角度傾斜させた軸周りの角速度を検出する第２のセンサと、前記倒立型移動体のピッチ軸周りの角速度を検出する第３のセンサのそれぞれが検出した角速度に基づいて倒立制御を行う倒立型移動体の制御方法であって、前記倒立型移動体のピッチ軸の加速度、前記倒立型移動体のロール軸の加速度、及び前記倒立型移動体のヨー軸の加速度を検出し、前記第１のセンサ及び前記第２のセンサのそれぞれで検出した角速度に基づいて算出した前記倒立型移動体のピッチ軸周りの第１角速度と、前記第３のセンサが検出した前記倒立型移動体のピッチ軸周りの第２角速度と、前記検出した前記倒立型移動体のピッチ軸、ロール軸、及びヨー軸のそれぞれの加速度に基づいて算出した前記倒立型移動体のピッチ軸周りの第３角速度の相互関係に基づいて、特定の安全機能を発動するものである。

上述した本発明の各態様によれば、故障検出精度を低下させることなく、コストを低減することができる倒立型移動体及びその制御方法を提供することができる。

実施の形態にかかる倒立二輪車の概要構成を示す図である。 実施の形態にかかる制御装置の構成を示すブロック図である。 実施の形態にかかるセンサが検出する角速度及び加速度を示す図である。 実施の形態で検出・算出されるＸ軸、Ｙ軸、及びＺ軸の加速度を示す図である。 実施の形態で検出・算出される角速度を示す図である。 実施の形態にかかるセンサ故障検出処理を示すフローチャートである。 実施の形態にかかる加速度及び角速度の遷移の一例を示す図である。

図１を参照して、本発明の実施の形態にかかる倒立二輪車１について説明する。図１は、本発明の実施の形態にかかる倒立二輪車１の概要構成を示す図である。

倒立二輪車１は、ステッププレート３に搭乗した搭乗者が、倒立二輪車１の前後方向に荷重を作用させた際における、前後方向への倒立二輪車１の姿勢角（ピッチ角）をセンサで検出し、この検出結果に基づいて、倒立二輪車１の倒立状態を維持するように左右の車輪２を駆動するモータを制御する。すなわち、倒立二輪車１は、ステッププレート３に搭乗した搭乗者が前方に荷重を作用させて倒立二輪車１を前方に傾斜させると、倒立二輪車１の倒立状態を維持するように前方に加速し、搭乗者が後方に荷重を作用させて倒立二輪車１を後方に傾斜させると、倒立二輪車１の倒立状態を維持するように後方に加速するように、左右の車輪２を駆動するモータを制御する。倒立二輪車１は、制御の安定性を確保するために、モータを制御する制御系が２重化されている。

なお、上述のモータの制御は、倒立二輪車１に搭載された制御装置１０によって行われる。次に、図２を参照して制御装置１０について説明する。

続いて、図２を参照して、本発明の実施の形態にかかる制御装置１０の構成について説明する。図２を参照して、本発明の実施の形態にかかる制御装置１０の構成を示すブロック図である。

制御装置１０は、マイクロコントローラ１１、１２（以下、「マイコン」とも呼ぶ）、インバータ１３〜１６、モータ１７、１８、及びセンサ１９〜２２を有する。

制御装置１０は、倒立二輪車１の制御の安定性を確保するために、１系のシステム１００と２系のシステム２００とに二重化された二重系システムとなっている。すなわち、通常時には両方のシステム１００、２００によって倒立二輪車１の制御を行い、片方のシステムで異常が検出された場合には、他方のシステムによって倒立二輪車１を安全停止させるように制御する。１系のシステム１００は、マイコン１１、インバータ１３、１４、及びセンサ１９〜２１を含む。２系のシステム２００は、マイコン１２、インバータ１５、１６、及びセンサ２２を含む。

以下、倒立二輪車１のロール軸をＸ軸とも呼び、倒立二輪車１のピッチ軸をＹ軸とも呼び、倒立二輪車１のヨー軸をＺ軸とも呼んで説明する。１系のシステム１００では、センサ１９とセンサ２０は、それぞれの検出軸が、ピッチ軸及びロール軸のそれぞれからヨー軸と直交する平面上で４５°の角度をなして対向するように配置されている。すなわち、センサ１９とセンサ２０は、それぞれの検出軸が、ピッチ軸に対して対称的となるように異なる向きに４５°傾斜するように配置される。また、１系のシステム１００では、センサ２１は、その検出軸がヨー軸と一致するように配置されている。２系のシステム２００では、センサ２２は、その検出軸がピッチ軸と一致するように配置されている。

マイコン１１、１２のそれぞれは、センサ１９〜２１及びセンサ２２のそれぞれから出力される角速度信号に基づいて、上述したように、倒立二輪車１が倒立状態を維持するようにモータ１７、１８を制御するＥＣＵ（Engine Control Unit）である。マイコン１１、１２のそれぞれは、ＣＰＵ（Central Processing Unit）及び記憶部を有し、記憶部に格納されたプログラムを実行することによって、本実施の形態におけるマイコン１１、１２のそれぞれとしての処理を実行する。すなわち、マイコン１１、１２のそれぞれの記憶部に格納されるプログラムは、本実施の形態におけるマイコン１１、１２のそれぞれにおける処理を、ＣＰＵに実行させるためのコードを含む。なお、記憶部は、例えば、このプログラムや、ＣＰＵにおける処理に利用される各種情報を格納することができる任意の記憶装置を含んで構成される。記憶装置は、例えば、メモリ等である。

マイコン１１は、モータ１７を制御する指令値を生成し、インバータ１３に出力する。また、マイコン１１は、モータ１８を制御する指令値を生成し、インバータ１４に出力する。ここで、マイコン１１は、センサ１９、２０から出力される角速度信号に基づいて倒立二輪車１の姿勢角を算出し、算出した姿勢角に基づいて倒立二輪車１の倒立状態を維持するようにモータ１７、１８を制御する指令値を生成する。

具体的には、マイコン１１は、センサ１９、２０のそれぞれから出力される角速度信号が示す角速度から、ピッチ軸周りの角速度を算出する。マイコン１１は、算出したピッチ軸周りの角速度を積分することで倒立二輪車１の前後方向の姿勢角（ピッチ角）を算出し、算出した姿勢角（ピッチ角）に基づいて倒立二輪車１の倒立状態を維持するように、モータ１７、１８を制御する指令値を生成する。ここで、センサ１９、２０のそれぞれから出力される角速度信号が示す角速度は、上述したようにピッチ軸から４５°傾斜した軸周りにおける角速度となる。そのため、マイコン１１は、後述するように、それらの角速度に対して回転行列計算を行うことで、ピッチ軸周りの角速度を算出し、算出したピッチ軸周りの角速度に基づいて倒立二輪車１の前後方向の姿勢角（ピッチ角）を算出する。

また、マイコン１１は、センサ１９、２０のそれぞれから出力される角速度信号が示す角速度から、ロール軸周りの角速度を算出する。マイコン１１は、算出したロール軸周りの角速度を積分することで倒立二輪車１の左右方向の姿勢角（ロール角）を算出し、算出した姿勢角（ロール角）に基づいて倒立二輪車１を旋回させるように、モータ１７、１８を制御する指令値を生成する。ここで、センサ１９、２０のそれぞれから出力される角速度信号が示す角速度は、ロール軸から４５°傾斜した軸周りにおける角速度ともなる。そのため、マイコン１１は、後述するように、それらの角速度に対して回転行列計算を行うことで、ロール軸周りの角速度を算出し、算出したロール軸周りの角速度に基づいて倒立二輪車１の左右方向の姿勢角（ロール角）を算出する。

また、マイコン１１は、センサ２１から出力される角速度信号が示すヨー軸周りの角速度に基づいて、任意の倒立二輪車１の制御を行うようにしてもよい。例えば、マイコン１１は、倒立二輪車１の急激な旋回を防止するために、センサ２１から出力される角速度信号が示す角速度が所定の角速度を超えていると判断した場合に、それ以上の角速度で倒立二輪車１が旋回しないように、モータ１７、１８を制御する指令値を生成するようにしてもよい。

マイコン１２は、モータ１７を制御する指令値を生成し、インバータ１５に出力する。また、マイコン１２は、モータ１８を制御する指令値を生成し、インバータ１６に出力する。ここで、マイコン１２は、センサ２２から出力される角速度信号に基づいて倒立二輪車１の姿勢角を算出し、算出した姿勢角に基づいて倒立二輪車１の倒立状態を維持するようにモータ１７、１８を制御する指令値を生成する。

具体的には、マイコン１２は、センサ２２から出力される角速度信号が示すピッチ軸周りの角速度を積分することで倒立二輪車１の前後方向の姿勢角（ピッチ角）を算出し、算出した姿勢角（ピッチ角）に基づいて倒立二輪車１の倒立状態を維持するように、モータ１７、１８を制御する指令値を生成する。

インバータ１３は、マイコン１１から出力された指令値に基づいてＰＷＭ（Pulse Width Modulation）制御を行うことで、指令値に従ったモータ制御を行う駆動電流を生成し、モータ１７に供給する。インバータ１４は、マイコン１１から出力された指令値に基づいてＰＷＭ制御を行うことで、指令値に従ったモータ制御を行う駆動電流を生成し、モータ１８に供給する。インバータ１５は、マイコン１２から出力された指令値に基づいてＰＷＭ制御を行うことで、指令値に従ったモータ制御を行う駆動電流を生成し、モータ１７に供給する。インバータ１６は、マイコン１２から出力された指令値に基づいてＰＷＭ制御を行うことで、指令値に従ったモータ制御を行う駆動電流を生成し、モータ１８に供給する。

モータ１７、１８のそれぞれは、二重巻のモータである。モータ１７は、インバータ１３から供給される駆動電流と、インバータ１５から供給される駆動電流とに基づいて駆動される。モータ１７を駆動することによって、倒立二輪車１の左側の車輪２が回転する。モータ１８は、インバータ１４から供給される駆動電流と、インバータ１６から供給される駆動電流とに基づいて駆動される。モータ１８を駆動することによって、倒立二輪車１の右側の車輪２が回転する。

次に、図３を参照してセンサ１９〜２２のそれぞれについて説明する。図３は、センサ１９〜２２のそれぞれが検出する角速度及び加速度を示す図である。ここで、センサ１９〜２１のそれぞれは、１軸の角速度と２軸の加速度を検出可能な複合型センサチップを含む。すなわち、センサ１９〜２１のそれぞれは、ジャイロセンサ及び加速度センサとして機能する。また、センサ２２は、１軸の角速度を検出するセンサチップである。すなわち、センサ２２は、ジャイロセンサとして機能する。

センサ１９は、上述の通り、ピッチ軸及びロール軸のそれぞれと４５°傾斜した軸周りの角速度ω０を検出し、検出した角速度ω０を示す角速度信号を生成してマイコン１１に出力する。また、センサ１９は、ピッチ軸及びロール軸のそれぞれと４５°傾斜した軸向の加速度Acc_leftを検出し、検出した加速度Acc_leftを示す傾斜軸加速度信号を生成してマイコン１１に出力する。角速度ω０の検出軸と加速度Acc_leftの検出軸は直交するように配置される。また、センサ１９は、Ｚ軸の加速度AccZ_0を検出し、検出した加速度AccZ_0を示すＺ軸加速度信号を生成してマイコン１１に出力する。

センサ２０は、上述の通り、ピッチ軸及びロール軸のそれぞれと４５°傾斜した軸周りの角速度ω１を検出し、検出した角速度ω１を示す角速度信号を生成してマイコン１１に出力する。また、センサ２０は、ピッチ軸及びロール軸のそれぞれと４５°傾斜した軸の加速度Acc_rightを検出し、検出した加速度Acc_rightを示す傾斜軸加速度信号を生成してマイコン１１に出力する。角速度ω１の検出軸と加速度Acc_ rightの検出軸は直交するように配置される。すなわち、角速度ω０の検出軸と加速度Acc_ rightの検出軸は同一軸となり、角速度ω１の検出軸と加速度Acc_leftの検出軸は同一軸となる。また、センサ２０は、Ｚ軸の加速度AccZ'_0を検出し、検出した加速度AccZ_0を示すＺ軸加速度信号を生成してマイコン１１に出力する。

センサ２１は、ヨー軸周りの角速度ω２を検出し、検出した角速度ω２を示す角速度信号を生成してマイコン１１に出力する。また、センサ２１は、Ｘ向の加速度AccX_0を検出し、検出した加速度AccX_0を示すＸ軸加速度信号を生成してマイコン１１に出力する。また、センサ２１は、Ｙ軸方向の加速度AccY_0を検出し、検出した加速度AccY_0を示すＹ軸加速度信号を生成してマイコン１１に出力する。

センサ２２は、ピッチ軸周りの角速度ω３を検出し、検出した角速度ω３を示す角速度信号を生成してマイコン１２に出力する。

続いて、図４を参照して、Ｘ軸、Ｙ軸、及びＺ軸のそれぞれにおける加速度の算出方法について説明する。図４は、本実施の形態において検出・算出されるＸ軸、Ｙ軸、及びＺ軸の加速度を示す図である。

上述したように、センサ２１は、その２つの加速度の検出軸のそれぞれが、Ｘ軸及びＹ軸と一致している。また、センサ１９、２０は、その１つの加速度の検出軸が、Ｚ軸と一致している。そのため、マイコン１１は、式（１）〜（３）に示すように、Ｘ軸及びＹ軸のそれぞれの加速度AccX、AccYを、センサ２１が検出したＸ軸及びＹ軸のそれぞれの加速度AccX_0、AccY_0とし、Ｚ軸の加速度AccZを、センサ１９が検出したＺ軸の加速度AccZ_0又はセンサ２０が検出したＺ軸の加速度AccZ'_0とする。なお、本実施の形態では、Ｚ軸の加速度AccZとして、センサ１９が検出したＺ軸の加速度AccZ_0を使用する場合について説明する。

すなわち、マイコン１１は、式（１）に示すように、センサ２１から出力されたＸ軸加速度情報が示すＸ軸の加速度AccX_0を、Ｘ軸の加速度AccXとする。また、マイコン１１は、式（２）に示すように、センサ２１から出力されたＹ軸加速度情報が示すＹ軸の加速度AccY_0を、Ｙ軸の加速度AccYとする。また、マイコン１１は、式（３）に示すように、センサ１９から出力されたＺ軸加速度情報が示すＺ軸の加速度AccZ_0を、Ｚ軸の加速度AccZとする。

また、マイコン１１は、後述するように、センサ１９〜２１の加速度検出に関する故障を検出するための比較に用いられる加速度AccX'_0、AccY'_0も算出する。ここで、上述したように、センサ１９、２０のそれぞれは、その１つの加速度の検出軸が、ピッチ軸及びロール軸のそれぞれに対して４５°の角度をなしている。そのため、マイコン１１は、式（４）に示すように、Ｘ軸の加速度AccX'_0を、センサ１９、２０のそれぞれが検出した加速度Acc_left、Acc_rightのそれぞれにおけるＸ軸成分の加速度を合成することで算出する。

具体的には、Ｘ軸の角速度AccX'_0は、式（４）に示すように、センサ１９が検出した加速度Acc0_leftとセンサ２０が検出した加速度Acc0_rightの合計値（加算値）から、２の平方根を除算することで算出される。すなわち、マイコン１１は、センサ１９から出力された傾斜軸加速度情報が示す加速度Acc0_leftと、センサ２０から出力された傾斜軸加速度情報が示す加速度Acc0_rightの合計値から、２の平方根を除算して、Ｘ軸の角速度AccX'_0を算出する。

また、マイコン１１は、式（５）に示すように、Ｙ軸方向の加速度AccY'_0を、センサ１９、２０のそれぞれが検出した加速度Acc_left、Acc_rightのそれぞれにおけるＹ軸成分の加速度を合成することで算出する。

具体的には、Ｙ軸の角速度AccY'_0は、式（５）に示すように、センサ１９が検出した加速度Acc0_leftとセンサ２０が検出した加速度Acc0_rightの差分値（減算値）から、２の平方根を除算することで算出される。すなわち、マイコン１１は、センサ１９から出力された傾斜軸加速度情報が示す加速度Acc0_leftと、センサ２０から出力された傾斜軸加速度情報が示す加速度Acc0_rightの差分値から、２の平方根を除算して、Ｙ軸の角速度AccY'_0を算出する。

続いて、図５を参照して、ロール軸及びピッチ軸における角速度の算出方法について説明する。図５は、本実施の形態において検出・算出される角速度を示す図である。

上述したように、１系のシステム１００におけるセンサ１９、２０のそれぞれは、その角速度の検出軸が、ピッチ軸及びロール軸のそれぞれに対して４５°の角度をなしている。そのため、マイコン１１は、式（６）に示すように、ロール軸周りの角速度Roll_0を、センサ１９、２０のそれぞれが検出した角速度ω０、ω１のそれぞれにおけるロール軸成分の角速度を合成することで算出する。

具体的には、１系のシステム１００において検出されるロール軸周りの角速度Roll_0は、式（６）に示すように、センサ１９が検出した角速度ω０とセンサ２０が検出した角速度ω１の差分値（減算値）から、２の平方根を除算することで算出される。すなわち、マイコン１１は、センサ１９から出力された角速度情報が示す角速度ω０と、センサ２０から出力された角速度情報が示す角速度ω１の差分値から、２の平方根を除算して、ロール軸周りの角速度Roll_0を算出する。

また、マイコン１１は、式（７）に示すように、ピッチ軸周りの角速度Pitch_0を、センサ１９、２０のそれぞれが検出した角速度ω０、ω１のそれぞれにおけるピッチ軸成分の角速度を合成することで算出する。

具体的には、１系のシステム１００において検出されるピッチ軸周りの角速度Pitch_0は、式（７）に示すように、センサ１９が検出した角速度ω０とセンサ２０が検出した角速度ω１の合計値（加算値）から、２の平方根を除算することで算出される。すなわち、マイコン１１は、センサ１９から出力された角速度情報が示す角速度ω０と、センサ２０から出力された角速度情報が示す角速度ω１の合計値から、２の平方根を除算して、ピッチ軸周りの角速度Pitch_0を算出する。

上述したように、２系のシステム２００におけるセンサ２２は、その角速度の検出軸がピッチ軸と一致している。そのため、マイコン１１は、式（８）に示すように、ピッチ軸周りの角速度Pitch_1を、センサ２２が検出した角速度ω３とする。

すなわち、マイコン１１は、式（８）に示すように、２系のシステム２００において検出されるピッチ軸周りの角速度Pitch_1を、センサ２２から出力された角速度情報が示す角速度ω３とする。

また、マイコン１１は、後述するように、センサ１９、２０、２２の角速度検出に関する故障を検出するための比較に用いられる角速度Pitch_Accも算出する。マイコン１１は、式（９）に示すように、ピッチ軸周りの角速度Pitch_Accを、Ｘ軸、Ｙ軸、及びＺ軸の加速度AccX、AccY、AccZに基づいて近似的に算出する。

具体的には、マイコン１１は、式（９）に示すように、Ｘ軸の加速度AccXの二乗、Ｙ軸の加速度AccYの二乗、及びＺ軸の加速度AccZの二乗の合計値（加算値）の平方根を、Ｘ軸の加速度AccXから除算した値の逆正弦関数の値を、ピッチ軸周りの角速度Pitch_Accとして算出する。

本実施の形態では、以上に説明したようにして得られたピッチ軸周りの角速度Pitch_0、Pitch_1、Pitch_Accに基づいて、ピッチ軸周りの角速度検出に関するセンサ故障の検出、及び、故障センサの特定を行う。このように、センサ１９、２０、２２によって検出された角速度に基づいて得られたピッチ軸周りの角速度Pitch_0、Pitch_1に加えて、加速度に基づいて得られたピッチ軸周りの角速度Pitch_Accも利用して追加的に検証を行うことで、故障センサの特定精度を向上することを可能としている。

続いて、図６を参照して、本発明の実施の形態にかかる制御装置１０のセンサ故障検出処理について説明する。図６を参照して、本発明の実施の形態にかかる制御装置１０のセンサ故障検出処理を示すフローチャートである。

本実施の形態におけるセンサ故障検出では、上述したように加速度に基づいて得られたピッチ軸周りの角速度Pitch_Accも利用する。そのため、まず、マイコン１１は、そのピッチ軸周りの角速度Pitch_Accを用いて、ピッチ軸周りの角速度検出に関するセンサ故障を判断する前に、加速度検出に関するセンサ故障を検出する処理を開始する（Ｓ１）。

マイコン１１は、センサ１９、２０のそれぞれから出力された傾斜軸加速度情報が示す加速度Acc0_left、Acc0_rightに基づいて、Ｘ軸の加速度AccX'_0を算出する。そして、マイコン１１は、センサ２１から出力されたＸ軸加速度情報が示すＸ軸の加速度AccXと、算出したＸ軸の加速度AccX'_0とを比較し、それぞれの加速度AccX、AccX'_0が所定範囲内で一致するか否かを判断する（Ｓ２）。ここで、「所定範囲内で一致」とは、完全一致する場合としてもよく、少し値が異なっていてもその差が所定値よりも小さい場合（完全一致を含む）としてもよい。

加速度AccX、AccX'_0が所定範囲内で一致していないと判断した場合（Ｓ２：ＹＥＳ）、センサ１９又はセンサ２０、もしくは、センサ２１において、ピッチ軸と４５°傾斜する軸又はＸ軸の加速度が正常に検出できない故障が発生していると判断する（Ｓ３）。この場合、マイコン１１は、倒立二輪車１に対して、加速度検出に関する異常に応じた所定の安全機能を発動する。例えばマイコン１１、１２は、倒立二輪車１を停止させるように制動制御を行う。具体的には、マイコン１１は、マイコン１２にセンサの故障を通知する信号を出力するとともに、その信号を受けたマイコン１２とともに倒立二輪車１を停止させるように指令値を生成してインバータ１３、１４に出力する。また、警告音を発しての警告や、物理的なブレーキを掛ける制動制御を行うようにしてもよい。

加速度AccX、AccX'_0が所定範囲内で一致していると判断した場合（Ｓ２：ＮＯ）、マイコン１１は、センサ１９、２０のそれぞれから出力された傾斜軸加速度情報が示す加速度Acc0_left、Acc0_rightに基づいて、Ｙ軸の加速度AccY'_0を算出する。そして、マイコン１１は、センサ２１から出力されたＹ軸加速度情報が示すＹ軸の加速度AccYと、算出したＹ軸の加速度AccY'_0とを比較し、それぞれの加速度AccY、AccY'_0が所定範囲内で一致するか否かを判断する（Ｓ４）。

加速度AccY、AccY'_0が所定範囲内で一致していないと判断した場合（Ｓ３：ＹＥＳ）、センサ１９又はセンサ２０、もしくは、センサ２１において、ピッチ軸と４５°傾斜する軸又はＹ軸の加速度が正常に検出できない故障が発生していると判断する（Ｓ５）。この場合、マイコン１１は、倒立二輪車１に対して、加速度検出に関する異常に応じた所定の安全機能を発動する。例えばマイコン１１は、上述したように倒立二輪車１を停止させる等する。

加速度AccY、AccY'_0が所定範囲内で一致していると判断した場合（Ｓ４：ＮＯ）、マイコン１１は、センサ１９から出力されたＺ軸加速度情報が示すＺ軸の加速度AccZと、センサ２０から出力されたＺ軸加速度情報が示すＺ軸の加速度AccZ'_0とを比較し、それぞれの加速度AccZ、AccZ'_0が所定範囲内で一致するか否かを判断する（Ｓ６）。

加速度AccZ、AccZ'_0が所定範囲内で一致していないと判断した場合（Ｓ６：ＹＥＳ）、センサ１９又はセンサ２０において、Ｚ軸方向の加速度が正常に検出できない故障が発生していると判断する（Ｓ７）。この場合、マイコン１１は、倒立二輪車１に対して、加速度検出に関する異常に応じた所定の安全機能を発動する。例えばマイコン１１は、上述したように倒立二輪車１を停止させる等する。

加速度AccZ、AccZ'_0が所定範囲内で一致していると判断した場合（Ｓ６：ＮＯ）、センサ１９〜２１による加速度検出が正常に行われていると判断する。そのため、マイコン１１は、ピッチ軸周りの角速度検出に関するセンサ故障を検出する処理を開始する（Ｓ８）。

マイコン１１は、センサ１９、２０のそれぞれから出力された角速度情報が示す角速度ω０、ω１に基づいて、ピッチ軸周りの角速度Pitch_0を算出する。また、マイコン１２は、センサ２２から出力された角速度情報をマイコン１１に出力する。マイコン１１は、算出したピッチ軸周りの角速度Pitch_0と、マイコン１２から出力された角速度情報が示すピッチ軸周りの角速度Pitch_1とを比較し、それぞれの角速度Pitch_0、Pitch_1が所定範囲内で一致するか否かを判断する（Ｓ９）。

角速度Pitch_0、Pitch_1が所定範囲内で一致していると判断した場合（Ｓ９：ＮＯ）、センサ１９、２０において角速度検出が正常に行われており、正常なピッチ軸周りの角速度Pitch_0、Pitch_1が得られていることになる（Ｓ１０）。そのため、マイコン１１、１２のそれぞれは、倒立二輪車１の倒立制御を維持する。すなわち、マイコン１１は、算出したピッチ軸周りの角速度Pitch_0に基づいて、倒立二輪車１の倒立状態を維持するように指令値を生成してインバータ１３、１４に出力する。また、マイコン１２は、センサ２２から出力された角速度情報が示すピッチ軸周りの角速度Pitch_1に基づいて、倒立二輪車１の倒立状態を維持するように指令値を生成してインバータ１５、１６に出力する。これによって、マイコン１１、１２は、倒立二輪車１を倒立制御する。

角速度Pitch_0、Pitch_1が所定範囲内で一致していないと判断した場合（Ｓ９：ＹＥＳ）、センサ１９又はセンサ２０において、ピッチ軸と４５°傾斜した軸又はピッチ軸の角速度が正常に検出できない故障が発生している可能性があると判断する。この場合、マイコン１１は、センサ２１から出力されたＸ軸加速度情報及びＹ軸加速度情報のそれぞれが示すＸ軸の加速度AccX及びＹ軸の加速度AccYと、センサ１９から出力されたＺ軸加速度情報が示すＺ加速度AccZとに基づいて、ピッチ軸周りの角速度Pitch_Accを算出する。そして、マイコン１１は、ピッチ軸周りの角速度Pitch_0とピッチ軸周りの角速度Pitch_Accとの差分値と、ピッチ軸周りの角速度Pitch_1とピッチ軸周りの角速度Pitch_Accとの差分値とを比較し、角速度Pitch_1と角速度Pitch_Accの差分値が、角速度Pitch_0と角速度Pitch_Accの差分値よりも大きいか否かを判定する（Ｓ１１）。

角速度Pitch_1と角速度Pitch_Accの差分値が、角速度Pitch_0と角速度Pitch_Accの差分値よりも大きい場合（Ｓ１１：ＹＥＳ）、２系のシステム２００のセンサ２１においてピッチ軸の角速度が正常に検出できない故障が発生していると判断する（Ｓ１２）。

この場合、マイコン１１は、２系のシステム２００における角速度検出に関する異常に応じた所定の安全機能を発動する。例えばマイコン１１は、上述の通りにして得たピッチ軸周りの角速度Pitch_0に基づいて倒立二輪車１の倒立を維持しつつ、上述したように倒立二輪車１を停止させるように制御する等する。具体的には、マイコン１１は、倒立二輪車１を停止させるように指令値を生成してインバータ１３、１４に出力する。また、マイコン１１は、２系のシステム２００のインバータ１５、１６からのモータ１７、１８への出力を遮断するようにしてもよい。これは、例えばインバータ１５、１６とモータ１７、１８の間にリレー回路を備えるようにして、リレー回路に対してインバータ１５、１６とモータ１７、１８の接続を遮断する信号を出力することで実現する。また、マイコン１１が、マイコン１２にセンサの故障を通知する信号を出力するようにして、マイコン１２が、その信号に応じて、上記のようにリレー回路に信号を出力する等して、２系のシステム２００からのモータ１７、１８の制御を抑止するようにしてもよい。また、これによれば、一方の系の故障が確定した場合に、倒立二輪車１を即座に停止させなくても、他方の系から倒立制御を維持することも可能である。

角速度Pitch_1と角速度Pitch_Accの差分値が、角速度Pitch_0と角速度Pitch_Accの差分値よりも大きくない場合（Ｓ１１：ＮＯ）、１系のシステム１００のセンサ１９又はセンサ２０においてピッチ軸と４５°傾斜する軸の角速度が正常に検出できない故障が発生していると判断する（Ｓ１３）。

この場合、マイコン１２は、１系のシステム１００における角速度検出に関する異常に応じた所定の安全機能を発動する。例えばマイコン１２は、上述の通りにして得たピッチ軸周りの角速度Pitch_1に基づいて倒立二輪車１の倒立を維持しつつ、上述したように倒立二輪車１を停止させるように制御する等する。具体的には、マイコン１１は、マイコン１２にセンサの故障を通知する信号を出力する。マイコン１２は、その信号に応じて、倒立二輪車１を停止させるように指令値を生成してインバータ１５、１６に出力する。また、マイコン１２は、１系のシステム１００のインバータ１３、１４からのモータ１７、１８への出力を遮断するようにしてもよい。これは、例えばインバータ１３、１４とモータ１７、１８の間にリレー回路を備えるようにして、リレー回路に対してインバータ１３、１４とモータ１７、１８の接続を遮断する信号を出力することで実現する。また、マイコン１１が、上記のようにリレー回路に信号を出力する等して、１系のシステム１００からのモータ１７、１８の制御を抑止するようにしてもよい。また、これによれば、一方の系の故障が確定した場合に、倒立二輪車１を即座に停止させなくても、他方の系から倒立制御を維持することも可能である。

ここで、上記処理では、一度のＳ１２又はＳ１３の判断で、その判断を確定するようにしてもよく、所定の時間の間、同一の判断が継続した場合に、その判断を確定するようにしてもよい。例えば図７に示すように、センサ１９又はセンサ２０においてピッチ軸と４５°傾斜する軸の角速度が正常に検出できない故障が発生し、センサ１９、２０のそれぞれが検出した角速度ω０、ω１に基づいて算出されるピッチ軸周りの角速度Pitch_0が正常時より大きくなってしまったとする（図７の４４１５[ｍｓｅｃ]）。この場合、ステップＳ１３の判断が行われることになる。そして、所定の時間を２０ｍｓｅｃとした場合、ステップＳ１３の判断が２０ｍｓｅｃ継続したときに（図７の４４３５[ｍｓｅｃ]）、ステップＳ１３の判断を確定する。

なお、ステップＳ１３の判断の確定後に、さらに、センサ１９又はセンサ２０のうち、いずれが故障しているかを特定したい場合には、マイコン１１は、ピッチ軸周りの角速度ω２に対して回転行列計算を行い、ピッチ軸と４５°傾斜した２つの軸のそれぞれの成分の角速度を算出し、算出した角速度のそれぞれと角速度ω０、ω１とを比較して、その差がより大きくなる角速度ω０又はω１を検出するセンサを故障センサとして特定するようにすればよい。

以上に説明したように、本実施の形態によれば、「ハの字」型に配置した２つのセンサ１９、２０によって検出した角速度から、ピッチ軸及びロール軸の角速度のそれぞれを合成して導出可能とするとともに、もう１つのセンサ２２によって検出したピッチ軸の角速度による比較によって、センサの故障及び故障したセンサの検出を可能としている。これによれば、それぞれがピッチ軸を検出軸として角速度を検出する３つのセンサで構成されるシステムと比較して、同じ故障診断能力を有するとともに、ロール軸の角速度も検出することができる。すなわち、ロール軸を検出軸として角速度を検出するセンサが不要となり、コストを低減することができる。

しかしながら、角速度に回転行列を掛けている場合には、それによって得られる角速度の分解能が下がってしまうという。例えば上述の例では角速度の検出軸が４５°傾斜しているため、それから得られるピッチ軸の角速度は０．７倍程度の値となる。よって、微小の変化量が大きな変化量として得られてしまう場合がある。そのため、これに対する考慮のない場合には、分解能に差のある角速度で比較検証が行われることになるため、故障検出精度が低下してしまうという問題がある。

それに対して、本実施の形態では、センサ１９、２０のそれぞれで検出した角速度に基づいて算出したピッチ軸周りの角速度と、センサ２２が検出したピッチ軸周りの角速度と、センサ１９、２１のそれぞれで検出した加速度に基づいて算出したピッチ軸周りの角速度の相互関係に基づいて、センサの異常（故障）を診断するようにしている。これによれば、さらに分解能が下がっていない加速度から算出した角速度による比較検証も行うことで、１系のセンサ１９、２０と、２系のセンサ２２のうち、いずれのセンサが故障しているかを特定するようにしているため、精度を落とすことなく、故障センサを特定することができる。すなわち、本実施の形態によれば、故障検出精度を低下させることなく、コストを低減することができる。

また、本実施の形態では、センサ１９、２０、２２のうち、１つのセンサ２２は、ピッチ軸を検出軸として角速度を検出するように配置されているため、センサ２２に関しては回転行列計算を行うことなく、ピッチ軸の角速度検出が可能である。よって、その分の処理を簡易にし、処理時間を低減することができる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。

上記実施の形態では、制御対象とする倒立型移動体が倒立二輪車である場合について例示したが、車輪数は、これに限られない。なお、車輪数が異なる場合、言うまでもなく、それに対応する構成要素（インバータ、モータ）数も変更される。

上記実施の形態では、センサ１９、２０の検出軸が、相互にピッチ軸を対称として異なる方向に同一の角度（４５°）傾斜した軸とした場合について例示したが、この角度に限られない。例えば４５°以外の角度であってもよく、センサ１９とセンサ２０の検出軸のそれぞれがピッチ軸から異なる角度で傾斜していてもよい。この場合、その角度に応じた回転行列計算を行うことでピッチ軸成分及びロール軸成分を抽出すればよいからである。

上記実施の形態では、センサ１９が検出したＺ軸の加速度AccZ_0を、Ｚ軸の加速度AccZとした場合について例示したが、センサ２０が検出したＺ軸の加速度AccZ'_0を、Ｚ軸の加速度AccZとしてもよい。この場合、ステップＳ６において、センサ２０から出力されたＺ軸加速度情報が示すＺ軸の加速度AccZと、センサ１９から出力されたＺ軸加速度情報が示すＺ軸の加速度AccZ_0とを比較するようにすればよい。

上記実施の形態では、角速度検出に関するセンサ故障検出として、ステップＳ９、Ｓ１１で説明した異常診断を行う場合について説明したが、センサ１９、２０が検出した角速度に基づいた角速度Pitch_0と、センサ２２が検出した角速度Pitch_1と、センサ１９、２１が検出した加速度に基づいた角速度Pitch_Accの相互関係に基づいてセンサの異常診断を行うものであれば、これに限られない。例えば、それぞれの角速度Pitch_0、Pitch_1、Pitch_Accが相互に所定範囲内で一致するか否かによる多数決比較を行うようにしてもよい。

１ 倒立二輪車
１０ 制御装置
１１、１２ マイコン
１３、１４、１５、１６ インバータ
１７、１８ モータ
１９、２０、２１、２２ センサ
１００、２００ システム

Claims (9)

1. 倒立制御される倒立型移動体であって、
   前記倒立型移動体のヨー軸に直交する平面上で前記倒立型移動体のピッチ軸から第１の所定角度傾斜させた軸周りの角速度を検出する第１のセンサと、
   前記倒立型移動体のヨー軸に直交する平面上で前記倒立型移動体のピッチ軸から第２の所定角度傾斜させた軸周りの角速度を検出する第２のセンサと、
   前記倒立型移動体のピッチ軸周りの角速度を検出する第３のセンサと、
   前記倒立型移動体のピッチ軸の加速度を検出するピッチ軸加速度検出部と、
   前記倒立型移動体のロール軸の加速度を検出するロール軸加速度検出部と、
   前記倒立型移動体のヨー軸の加速度を検出するヨー軸加速度検出部と、
   前記第１のセンサ、前記第２のセンサ、及び前記第３のセンサのそれぞれが検出した角速度に基づいて前記倒立制御を行う制御部と、を備え、
   前記制御部は、前記第１のセンサ及び前記第２のセンサのそれぞれで検出した角速度に基づいて算出した前記倒立型移動体のピッチ軸周りの第１角速度と、前記第３のセンサが検出した前記倒立型移動体のピッチ軸周りの第２角速度と、前記ピッチ軸加速度検出部、前記ロール軸加速度検出部、及び前記ヨー軸加速度検出部のそれぞれで検出した加速度に基づいて算出した前記倒立型移動体のピッチ軸周りの第３角速度の相互関係に基づいて、特定の安全機能を発動する、
   倒立型移動体。
2. 前記制御部は、前記第１のセンサ及び前記第２のセンサのそれぞれが検出した角速度に基づいて前記倒立制御を行う第１の制御部と、前記第３のセンサが検出した角速度に基づいて前記倒立制御を行う第２の制御部と含み、
   前記制御部は、前記第３のセンサが異常であると判断した場合、前記第１の制御部及び前記第２の制御部のうち、前記第１の制御部によって前記倒立型移動体の倒立制御を行い、
   前記制御部は、前記第１のセンサ又は前記第２のセンサが異常であると判断した場合、前記第１の制御部及び前記第２の制御部のうち、前記第２の制御部によって前記倒立型移動体の倒立制御を行う、
   請求項１に記載の倒立型移動体。
3. 前記安全機能は、前記倒立型移動体を停止させる制動機能である、
   請求項１又は２に記載の倒立型移動体。
4. 前記第１のセンサは、さらに前記倒立型移動体のピッチ軸から第２の所定角度傾斜させた軸の加速度を検出し、
   前記第２のセンサは、さらに前記倒立型移動体のピッチ軸から第１の所定角度傾斜させた軸の加速度を検出し、
   前記倒立型移動体は、前記ヨー軸加速度検出部を第１のヨー軸加速度検出部として有するとともに、さらに前記倒立型移動体のヨー軸の加速度を検出する第２のヨー軸加速度検出部を有し、
   前記制御部は、
   前記第１のセンサ及び前記第２のセンサのそれぞれで検出した加速度に基づいて算出した前記倒立型移動体のピッチ軸の加速度と、前記ピッチ軸加速度検出部が検出した前記倒立型移動体のピッチ軸の加速度の比較、
   前記第１のセンサ及び前記第２のセンサのそれぞれで検出した加速度に基づいて算出した前記倒立型移動体のロール軸の加速度と、前記ロール軸加速度検出部が検出した前記倒立型移動体のロール軸の加速度の比較、及び、
   前記第１のヨー軸加速度検出部が検出した前記倒立型移動体のヨー軸の加速度と、前記第２のヨー軸加速度検出部が検出した前記倒立型移動体のヨー軸の加速度の比較を行い、
   それらの比較結果に基づいて、前記安全機能を発動する、
   請求項１乃至３のいずれか１項に記載の倒立型移動体。
5. 前記第１のセンサは、前記第１のヨー軸加速度検出部を有し、
   前記第２のセンサは、前記第２のヨー軸加速度検出部を有し、
   前記倒立型移動体は、前記ピッチ軸加速度検出部及び前記ロール軸加速度検出部を有する第４のセンサを有する、
   請求項４に記載の倒立型移動体。
6. 前記制御部は、前記第１角速度と前記第２角速度が所定範囲内で一致しない場合、前記第１角速度と前記第３角速度の差分値となる第１差分値と、前記第２角速度と前記第３角速度の差分値となる第２差分値を比較し、
   前記第２差分値が前記第１差分値よりも大きい場合、前記第３のセンサが異常であると判断し、
   前記第２差分値が前記第１差分値よりも大きくない場合、前記第１のセンサ又は前記第２のセンサが異常であると判断する、
   請求項２に記載の倒立型移動体。
7. 前記制御部は、所定の時間の間、前記第１角速度と前記第２角速度が所定範囲内で一致せず、かつ、前記第１差分値と前記第２差分値の比較結果が一致し続ける場合、前記第１のセンサ又は前記第２のセンサ、もしくは、前記第３のセンサが異常であると判断する、
   請求項６に記載の倒立型移動体。
8. 前記第１の所定角度及び前記第２の所定角度のそれぞれは、相互にピッチ軸を対称として異なる方向に同一角度傾斜した角度となる、
   請求項１乃至７のいずれか１項に記載の倒立型移動体。
9. 倒立型移動体のヨー軸に直交する平面上で前記倒立型移動体のピッチ軸から第１の所定角度傾斜させた軸周りの角速度を検出する第１のセンサと、前記倒立型移動体のヨー軸に直交する平面上で前記倒立型移動体のピッチ軸から第２の所定角度傾斜させた軸周りの角速度を検出する第２のセンサと、前記倒立型移動体のピッチ軸周りの角速度を検出する第３のセンサのそれぞれが検出した角速度に基づいて倒立制御を行う倒立型移動体の制御方法であって、
   前記倒立型移動体のピッチ軸の加速度、前記倒立型移動体のロール軸の加速度、及び前記倒立型移動体のヨー軸の加速度を検出し、
   前記第１のセンサ及び前記第２のセンサのそれぞれで検出した角速度に基づいて算出した前記倒立型移動体のピッチ軸周りの第１角速度と、前記第３のセンサが検出した前記倒立型移動体のピッチ軸周りの第２角速度と、前記検出した前記倒立型移動体のピッチ軸、ロール軸、及びヨー軸のそれぞれの加速度に基づいて算出した前記倒立型移動体のピッチ軸周りの第３角速度の相互関係に基づいて、特定の安全機能を発動する、
   制御方法。

Images