JP4605227B2 - 転倒防止制御装置 - Google Patents

Description

本発明は、例えば二輪車や二足歩行ロボットのように左右に傾き自在な本体を転倒しないようにバランス制御する転倒防止制御装置に関するものである。

特許文献１には、操舵部と、操舵部によって操舵可能な前輪と、後輪と、前輪（ハンドル）の向きに応じて揺動するフライホイールと、フライホイールを駆動させる第１駆動部と、後輪を駆動させる第２駆動部とを備えた二輪車走行玩具が提案されている。この二輪車では、前輪の向きに応じてフライホイールの向きを変えることによって、フライホイールによるジャイロ効果によって、走行中に転倒し難くしたものである。

しかしながら、前記二輪車走行玩具の場合、前輪の向きに応じてフライホイールの向きを変えるに過ぎないので、通常走行中であればハンドル操作によって転倒は防止できても、停止時や微速進行時には、ハンドル操作だけでバランスを取ることは困難であり、転倒を有効に防止できないという問題がある。

特許文献２では、傾き検出センサで検出した傾きを制御回路に入力し、制御回路にてモータを駆動し、モータで慣性の大きな回転体を回転させて傾きを補正すべき方向と逆方向に回転体の回転数を上げて反作用偶力を発生させることにより、転倒を防止する倒立制御玩具が提供されている。この倒立制御玩具では、回転体の回転を制御することでバランスを取るため、停止時や微速進行時でも転倒を防止することが可能である。

前記倒立制御玩具では、玩具の傾きを検出する傾き検出センサとして、発光素子から送出されて床面で反射された光を受光素子で受光することにより、傾きを検出する光センサを用いている。しかしながら、実際には傾きを正確に測定することは容易ではない。すなわち、発光素子と受光素子とを用いた傾きセンサの場合、光を反射すべき床面が平坦であれば問題はないが、床面に凹凸があったり、両側部に床面が存在しない場合（幅狭な橋の上を渡る場合など）には、傾きを正確に検出できない。

さらに、前記倒立制御玩具では、真っ直ぐに立った状態の受光量を標準値として差分を取ることで傾きを検出しているが、真っ直ぐに立った状態（鉛直方向）がバランスの取れた状態とは限らない。例えば、玩具の重心位置が中心位置より左右にずれている場合や、横風を受けている場合には、鉛直方向に対してやや傾いた状態がバランスの取れた状態であり、そのバランスの取れた状態（角度）を標準位置とすべきであるのに拘わらず、上記の方法では鉛直方向を標準位置としているため、バランスを保つことができず、転倒してしまう可能性がある。

ところで、本体の傾きを検出する方法として、角速度センサを用いて角速度を検出し、その検出値を積分することで、傾きを推定することも可能である。しかしながら、角速度出力を積分する方法では、ノイズやオフセットが累積し、傾斜角推定と転倒防止制御とを継続できなくなるという問題が発生する。さらに、他の傾き検出装置としてウエイトを用いた傾きセンサもあるが、この場合もバランス状態に対する傾きは検出できず、しかも応答性が悪く、瞬時に傾きを検出できないという欠点がある。
特開２００３−１９０６５４号公報 特開平１１−４７４５４号公報

そこで、本発明の目的は、バランスの取れた状態からの傾斜角を正確に推定でき、かつノイズやオフセットが累積せず、傾斜角推定と転倒防止制御とを継続できる転倒防止制御装置を提供することにある。

上記目的を達成するため、本発明は、左右に傾き自在な本体と、前記本体に略前後方向に検出軸を向けて取り付けた角速度センサと、前記本体に略前後方向に回転軸を向けて取り付けたモータと、前記モータの回転位置または回転速度を検出する回転センサと、前記モータの回転軸に連結された慣性ロータとを備え、前記モータで慣性ロータを回転させ、前記慣性ロータの回転に伴う反動トルクを利用して前記本体の傾きを補正する転倒防止制御装置であって、前記角速度センサの角速度出力ω₁ と前記モータに与えるトルク指令τ₀ とから、釣合い状態に対する前記本体の傾斜角を推定する傾斜角推定手段を備え、前記傾斜角推定手段により推定された傾斜角推定値を用いて前記本体の傾きを補正することを特徴とするものである。

本発明に係る転倒防止制御装置の作動原理は、特許文献２と同様に、モータで慣性ロータを回転させ、前記慣性ロータの回転に伴う反動トルクを利用して前記本体の傾きを補正するものであるが、補正に際して傾斜角を精度よく検出する必要がある。本発明では、傾斜角をセンサで直接検出するのではなく、また角速度センサの角速度出力を積分して求めるものでもない。すなわち、角速度センサの角速度出力ω₁ とモータに与えるトルク指令τ₀ とから傾斜角を推定する。ここで、傾斜角とは、重力によるトルク、およびカーブ走行による遠心力や、横風などによる外乱トルクの合計がゼロになる釣合い状態における本体の姿勢からの偏差角度である。傾斜角推定値を用いて慣性ロータの回転を制御し、傾斜角が０に収束するようにモータトルク制御を繰り返す。例えば、本体のバランス軸に対して、傾斜角が本体正面からみて左方向の場合は、バランス姿勢を維持するためには、本体正面からみて左回転方向に慣性ロータを加速する。逆に、傾斜角が本体正面からみて右方向の場合は、バランス姿勢を維持するためには、本体正面からみて右回転方向に慣性ロータを加速する。

本発明では、本体の傾斜角を検出するために、傾き検出センサを用いないので、床面に凹凸がある場合や、平均台のように両側部に床面が存在しない場合でも、正確に傾きを推定できる。また、角速度センサの角速度出力を積分する必要がないため、角速度センサの出力にノイズやオフセットが含まれる場合でも、傾斜角推定を継続でき、転倒防止制御を継続できる。さらに、従来のウエイトを用いた傾きセンサに比べて遙に応答性がよく、傾きを精度よく推定できる。このように本発明では、バランス軸からの本体の傾斜角を高精度にかつ応答よく推定できるので、この傾斜角に応じてモータに加えるトルクを高精度に制御できる。モータによって慣性ロータに加えられるトルクの反動トルクにより、本体の転倒を防ぐ方向に本体の傾斜角を精度よく制御することができる。その結果、停止状態や微速進行状態でも倒れない不倒体を作ることができる。

好ましい実施の形態によれば、目標傾斜角から前記傾斜角推定値を減じた傾斜角偏差信号を用いて、傾斜角速度指令ω₂ を生成する傾斜角速度指令生成手段と、前記傾斜角速度指令ω₂ から前記角速度センサの角速度出力ω₁ を減じた傾斜角速度偏差信号ω₂ −ω₁ を用いて、前記モータに与えるトルク指令τ₀ を生成するトルク指令生成手段と、を備えるのが望ましい。本発明では、まず目標傾斜角を設定し、この目標傾斜角から傾斜角推定値を差し引いて傾斜角偏差信号を求め、この偏差信号から本体に対する傾斜角速度指令ω₂ を生成する。次に、傾斜角速度指令ω₂ から角速度センサの角速度出力ω₁ を減じた傾斜角速度偏差信号ω₂ −ω₁ を用いて、モータに与えるトルク指令τ₀ を生成することができる。

好ましい実施の形態によれば、傾斜角推定値から本体を倒そうとする外部トルクを推定する外部トルク推定手段と、外部トルク推定値τ₃ を用いてトルク指令τ₀ を外部トルクが相殺される方向に補正するトルク補正手段と、を備えるのがよい。外部トルクとは、本体がバランス軸から傾くことによって本体に加わる重力、および外乱による傾き方向のトルクのことである。外部トルクをフィードフォワード制御で補償することにより、傾斜角ループと、傾斜角速度ループの応答周波数が低い場合でも、転倒防止制御を継続することができるため、安定な制御が可能となる。

好ましい実施の形態によれば、目標傾斜角を、モータの回転速度を用いて、かつ回転速度が減少する方向に生成する目標傾斜角生成手段を備えるのがよい。重力トルクを利用して慣性ロータのもつ角運動量を放出できるため、モータの回転速度が飽和することがなく、制御を継続できる。

本発明に係る転倒防止制御装置を、自立走行二輪車に適用することができる。この二輪車は、操舵部と、操舵部によって操舵可能な前輪と、後輪と、後輪を駆動させる後輪駆動部と、前輪および後輪を回転自在に支持するフレームとを持ち、この二輪車の転倒防止制御に本発明を適用することで、走行中は勿論、停止時や微速進行時でも倒れない二輪車を提供できる。なお、本発明の転倒防止制御を停止時や微速進行時のみ使用し、走行中は慣性ロータを回転させずに、操舵部を操作することによりバランスを取ることが可能である。

以上のように、本発明によれば、角速度センサの角速度出力とモータトルク指令とから釣合い状態に対する傾斜角を推定するようにしたので、従来の傾き検出センサを用いた場合と異なり、床面に凹凸があったり、平均台のように周囲に床面が存在しなくても、あるいは床面が若干傾斜していたとしても、正確に釣り合い状態に対する傾斜角を推定できる。また、角速度センサの角速度出力を積分する必要がないため、角速度センサの出力にノイズやオフセットが含まれる場合でも、傾斜角推定を継続でき、転倒防止制御を継続できる。さらに、従来のウエイトを用いた傾きセンサに比べて遙に応答性がよく、傾きを精度よく推定できる。したがって、モータトルクに加えるトルクを高精度に制御でき、停止状態や微速進行状態でも倒れない転倒防止制御装置を実現できる。

以下に、本発明の好ましい実施の形態を図面を参照して説明する。

（第１実施形態）
図１〜図３は本発明にかかる転倒防止制御装置を自転車ロボットに適用した第１実施形態を示す。

この自転車ロボットＡは、操舵用ハンドル１と、操舵用ハンドル１によって操舵可能な前輪２と、後輪３と、後輪３を駆動させる後輪駆動モータ４と、前輪２および後輪３を回転自在に支持するフレーム５と、フレーム５上に搭載された人形６を備えている。フレーム５には、傾斜角速度を測定するジャイロセンサ（角速度センサ）７がその検出軸を自転車Ａの略前後方向に向けて取り付けられている。人形６の胸部には慣性ロータ８と、慣性ロータ８を駆動するバランス用モータ９と、バランス用モータ９の回転角度を測定するエンコーダ１０とが取り付けられている。慣性ロータ８およびモータ９の回転軸も自転車Ａの略前後方向に向けて取り付けられている。ここで、略前後方向とは、厳密な前後方向に対して上下に若干角度ずれていてもよい。人形６の背中には、バランス用モータ９を制御する制御基板１１および電池１２が装備されている。制御基板１１には、モータ９を駆動するドライバ、Ａ／Ｄコンバータ、Ｄ／Ａコンバータ、カウンタ、コントローラなどが搭載されている。

通常の走行中は、ハンドル１を操舵することでバランスをとり、転倒を防止する。一方、停止状態や微速進行状態では、ハンドル１の操舵だけでバランスをとることは困難であるため、慣性ロータ８を駆動するときの反動を利用してバランスをとるように制御している。

自転車ロボットＡは図３に示す制御ブロックによって制御される。この制御ブロックは制御基板１１に格納されたブロックの一例である。まず、カウンタ部２０では、エンコーダ１０の出力パルスが計数される。モータ速度計算部２１では、カウンタ部２０の出力を回転角度に変換したあと、微分してバランス用モータ９の回転速度を求める。ノイズ除去のためのＬＰＦ（ローパスフィルタ）を装備してもよい。

目標傾斜角生成部２２は、バランス用モータ９の回転速度が自転車正面から見て左回転の場合は目標傾斜角を自転車正面から見て右方向に、バランス用モータ９の回転速度が自転車正面から見て右回転の場合は目標傾斜角を自転車正面から見て左方向になるように、バランス用モータ９の回転速度に比例係数をかけて求める。なお、積分器を追加して、慣性ロータ８に定常回転が残留しないようにするのがよい。

一方、Ａ／Ｄ部２３では、ジャイロセンサ７の角速度出力を測定する。傾斜角速度計算部２４では、角速度出力に変換係数をかけて、傾斜角速度ω₁ を計算する。

傾斜角推定部２５では、傾斜角速度ω₁ とモータトルク指令τ₂ とから、自転車本体（慣性ロータ以外の部分）と慣性ロータ８を含んだ系の傾斜角方向の運動方程式に基づき導出された、後述の（式１８）で表される傾斜角を計算し、さらに適切な推定速度を持たせてループを安定化させるための一次遅れ要素を直列に加えることで、傾斜角推定値を算出している。具体的には、（式１８）を用いた計算値に、一次遅れ要素として例えば１／（０．１Ｓ＋１）を直列に加えることになるが、これに限られるものではなく、適切な推定速度となるような任意の遅れ要素を加えることができる。ここで、傾斜角とは、重力によるトルク、およびカーブ走行による遠心力や横風などによる外乱トルクの合計がゼロになる釣合い状態における本体の姿勢からの偏差角度である。

補正トルク指令生成部２６では、傾斜角推定値に変換係数をかけて、自転車に働いている外部トルク推定値を算出し、補正トルク（＝外部トルク推定値）τ₃ を生成する。

目標傾斜角速度生成部２７では、目標傾斜角と傾斜角推定値の偏差に比例ゲインをかけて、目標傾斜角速度ω₂ を生成する。

トルク指令生成部２８では、目標傾斜角速度ω₂ と傾斜角速度ω₁ の偏差に対して、例えばＰＩ制御によりトルク指令τ₀ を生成する。モータトルク指令電圧計算部２９では、トルク指令τ₀ と補正トルクτ₃ とを加えたモータトルク指令τ₂ に対して、変換係数をかけて、指令電圧を生成する。最後に、Ｄ／Ａ部３０ではドライバに指令電圧を出力し、バランス用モータ９の回転を制御する。

ここで、（式１８）で表わされる推定傾斜角の計算式の導出方法について、以下に説明する。

図４は、慣性ロータ８を含む自転車ロボットＡを前方から見たモデルを示す。まずラグランジュ方程式により、運動方程式を導出する。自転車本体（慣性ロータ以外の部分）と慣性ロータ８を合わせた、全体の運動エネルギーＴと位置エネルギーＵは以下のようになる。

一般化座標と一般化速度による微分量は、以下のようになる。

ラグランジュ方程式（式９）、（式１０）に（式３）〜（式８）を代入する。

この結果、運動方程式として、次の（式１１）、（式１２）を得る。

（式１２）を変形すると、（式１３）となる。

これを（式１１）に代入し、sinθ₁ をθ₁ で近似すると、

を得る。（式１４）より、本体の運動は、慣性ロータ８の角度と角速度には無関係となる。

−本体傾斜角の推定−
本体傾斜角は、ジャイロセンサ７の出力の積分で求めることも可能であるが、偏差が累積して不正確になるため、別の方法で求める必要がある。そこで、モデルの運動方程式を用いて、ジャイロセンサ７が出力する本体傾斜角速度測定値と、モータトルクから、現在の傾斜角を推定する。運動方程式（式１４）を変形すると、

となる。

一方、ジャイロセンサ７が出力する本体傾斜角速度測定値をω₁ とすると、

であり、また外乱トルクτ₁ があるときの、見かけの釣り合い傾斜角度は、

である。従って、見かけの釣り合い傾斜角度に対する現在傾斜角度の偏差は、（式１５）より、

で推定することが出来る。ただし、適切な推定速度を持たせてループを安定化させるために、一次遅れ要素を直列に加えておくのがよい。

−外部トルクフィードフォワード−
（式１８）で推定された偏差角度によって、外部トルクを補償する。

として、トルクに加えておく。

とすれば、運動方程式（式１４）は、

となるから、外部トルクを補償できる。

−目標傾斜角度生成−

されていく。モータの回転速度にリミットがあるため、蓄積された回転速度を重力トルクを利用して放出するように、位置制御による補償を行う必要がある。そこで、目標傾斜角を以下のように決定した。

重力トルクにより回転速度を放出している間、仮に、傾斜角が一定であったとすると、

であるから、運動方程式（式１４）、（式１３）は、それぞれ（式２３）、（式２４）となる。

であるから、（式２４）と（式２５）を比較して、

であることが求められる。したがって、位置ループの目標値（目標傾斜角）として、（式２７）を設定すればよい。

放出時間Ｔ_A は、例えばＴ_A ＝１sec などとすればよい。

傾斜角度推定部２５は、原理的に定常偏差が残らないので、目標傾斜角度の生成には積分要素は必要ないが、実際には、慣性ロータ８に低速の定常回転が残る場合がある。これは、Ｄ／Ａのオフセットが原因と考えられる。そのままでも問題ないが、目標傾斜角度の生成部分に時定数１０秒程度の積分器を追加すれば、低速の定常回転を解消できる。

上記の原理に基づいた慣性ロータを含む自転車ロボットの安定性について測定した結果を図５〜図７に示す。図５〜図７は、外乱が印加されていない状態にある自転車ロボットに、指で本体を横向きに押して外乱が印加されたときの応答を示し、図５はジャイロセンサによる本体角速度、図６はモータトルク指令（定格トルク／３Ｖ）、図７は本体傾斜角推定値を示す。なお、サンプリング時間は１ｍｓである。

図７より明らかなように、外乱が印加されるまでの傾斜角推定値は±０．０５ｄｅｇ以内に安定して維持されており、安定したバランス状態が維持できていることがわかる。さらに、外乱を与えた場合でも、速やかに安定位置に収束していることがわかる。この実験結果から、本実施形態にかかる自転車ロボットは、不倒停止が可能で、外乱（定常的なステップ状外乱を含む）に対しても対応できることが確認された。

以下に、本発明の効果を列記する。
（１）ジャイロセンサ７の角速度出力を積分せずに、モデルベースで傾斜角を推定するため、ジャイロセンサ７の出力にノイズやオフセットが含まれる場合でも、傾斜角推定を継続でき、自転車の転倒防止制御を継続できる。従って、停止状態や微速進行状態でも倒れない自転車を作ることが出来る。
（２）ジャイロセンサ７の出力に基づいて傾斜角を推定し、バランス用モータで慣性ロータ８へ加えるトルクの反動を利用して、自転車の傾斜角を制御することができる。従って、停止状態や微速進行状態でも倒れない自転車を作ることが出来る。
（３）傾斜角推定は、釣り合い位置からの傾斜角を求めるので、重力トルクに加えて、カーブ走行中の遠心力などの外乱トルクがある場合でも、常に釣り合い位置からの傾斜角度によって生じる外部トルクを推定できるので、これを相殺するような補正トルクを算出することができ、外乱トルクがある場合でも本体のバランスを保つことができる。
（４）外部トルクをフィードフォワード制御で補償することにより、傾斜角ループと、傾斜角速度ループの応答周波数が低い場合でも、転倒防止制御を継続することができるため、安定な制御が可能となる。
（５）慣性ロータの回転速度の飽和を防止するように目標傾斜角度を生成するようにしたので、モータ回転速度が飽和する前に傾斜角を変化させ、重力トルクを利用して慣性ロータ８のもつ角運動量を放出できる。このため、停止状態や微速進行状態でも、転倒防止制御を継続可能な制御装置を製作することが出来る。

さらに詳しく述べると、以下の通りである。
傾斜角が自転車正面から見て左方向の場合は、その姿勢を維持するためには、自転車正面から見て左回転方向に慣性ロータ８を加速する必要がある。傾斜角が自転車正面から見て右方向の場合は、その姿勢を維持するためには、自転車正面から見て右回転方向に慣性ロータ８を加速する必要がある。このことを利用し、モータ回転速度が大きくなったときには、積極的に姿勢を傾斜させて、重力トルクを利用して慣性ロータ８のもつ角運動量を放出し、モータ回転速度を落とすことができる。慣性ロータ８を回転軸に取り付けたことで、モータ回転速度が飽和するまでの時間に余裕があるため、このような制御が可能となる。

目標傾斜角生成は、モータ回転速度が自転車正面から見て左回転の場合は目標傾斜角を自転車正面から見て右方向に、モータ回転速度が自転車正面から見て右回転の場合は目標傾斜角を自転車正面から見て左方向になるように、モータ回転速度に比例係数をかけて求める。また、更に積分器を追加しているため、Ｄ／Ａコンバータのオフセットなどが原因で、定常回転が残留することがない。

上記実施形態では、自転車ロボットの転倒防止制御について説明したが、本発明はこれのみに限定されるものではなく、例えば特許文献２に示されるような倒立玩具や、二足歩行ロボットなどの転倒防止制御にも適用できる。すなわち、二足歩行ロボットの場合、バランス軸からの傾斜角を推定することで、常に安定した歩行を実現できる。さらに、本発明はバイクなどの二輪車の一旦停止時における転倒防止制御にも適用できる。また、（１８）式で傾斜角偏差を推定する計算式を示したが、これは一例を示すに過ぎず、対象モデルによって傾斜角偏差の推定式は（１８）式と異なることがあり得る。

本発明にかかる転倒防止制御装置を適用した自転車ロボットの一実施形態の斜視図である。 自転車ロボットの側面図である。 自転車ロボットの制御ブロック図である。 自転車ロボットの正面からみたモデル図である。 外乱印加時のジャイロセンサによる本体角速度測定値である。 外乱印加時のモータトルク指令である。 外乱印加時の本体傾斜角度推定値である。

符号の説明

Ａ 自転車ロボット（本体）
１ 操舵用ハンドル（操舵部）
２ 前輪
３ 後輪
４ 後輪駆動モータ（後輪駆動部）
５ フレーム
６ 人形
７ ジャイロセンサ（角速度センサ）
８ 慣性ロータ
９ バランス用モータ
１０ エンコーダ（回転センサ）
１１ 制御基板
１２ 電池
２０ カウンタ部
２１ モータ速度計算部
２２ 目標傾斜角生成部
２３ Ａ／Ｄ部
２４ 傾斜角速度計算部
２５ 傾斜角推定部
２６ 補正トルク指令生成部
２７ 目標傾斜角速度生成部
２８ トルク指令生成部
２９ モータトルク指令電圧計算部
３０ Ｄ／Ａ部
Ｏ_１ 接地点
Ｏ_２ 慣性ロータ中心
ｍ_１ 本体質量
ｍ_２ 慣性ロータ質量
Ｉ_１ Ｏ_１まわりの本体の慣性モーメント
Ｉ_２ Ｏ_２まわりの慣性ロータの慣性モーメント
θ_１ 垂直軸に対する本体の傾斜角度
θ_２ 本体に対する慣性ロータの回転角度
τ_１ 本体に働くＯ_１まわりの外乱トルク
τ_２ 慣性ロータに働くモータトルク
ｌ_Ｇ Ｏ_１から本体重心位置までの距離
ｌ Ｏ_１からＯ_２までの距離離
ｇ 重力加速度

Claims (5)

1. 左右に傾き自在な本体と、前記本体に略前後方向に検出軸を向けて取り付けた角速度センサと、前記本体に略前後方向に回転軸を向けて取り付けたモータと、前記モータの回転位置または回転速度を検出する回転センサと、前記モータの回転軸に連結された慣性ロータとを備え、前記モータで慣性ロータを回転させ、前記慣性ロータの回転に伴う反動トルクを利用して前記本体の傾きを補正する転倒防止制御装置であって、
   前記角速度センサの角速度出力ω₁ と前記モータに与えるトルク指令τ₀ とから、釣合い状態に対する前記本体の傾斜角を推定する傾斜角推定手段を備え、前記傾斜角推定手段により推定された傾斜角推定値を用いて前記本体の傾きを補正することを特徴とする転倒防止制御装置。
2. 目標傾斜角から前記傾斜角推定値を減じた傾斜角偏差信号を用いて、傾斜角速度指令ω₂ を生成する傾斜角速度指令生成手段と、前記傾斜角速度指令ω₂ から前記角速度センサの角速度出力ω₁ を減じた傾斜角速度偏差信号ω₂ −ω₁ を用いて、前記モータに与えるトルク指令τ₀ を生成するトルク指令生成手段と、を備えたことを特徴とする、請求項１に記載の転倒防止制御装置。
3. 前記傾斜角推定値から前記本体を倒そうとする外部トルクを推定する外部トルク推定手段と、外部トルク推定値τ₃ を用いて前記トルク指令τ₀ を前記外部トルクが相殺される方向に補正するトルク補正手段と、を備えたことを特徴とする、請求項２に記載の転倒防止制御装置。
4. 前記目標傾斜角を、前記モータの回転速度を用いて、かつ前記回転速度が減少する方向に生成する目標傾斜角生成手段を備えたことを特徴とする、請求項２または３に記載の転倒防止制御装置。
5. 前記本体は、操舵部と、前記操舵部によって操舵可能な前輪と、後輪と、前記後輪を駆動させる後輪駆動部と、前記前輪および後輪を回転自在に支持するフレームとを持つ二輪車であることを特徴とする請求項１ないし４のいずれか１項に記載の転倒防止制御装置。

Images