JP4743347B2 - 転倒防止制御装置及びコンピュータプログラム - Google Patents

Description

本発明は、一輪車であってもピッチ方向への転倒を防止することができる転倒防止制御装置及びコンピュータプログラムに関する。

従来、二輪車、二足歩行ロボット等は、移動する場合に本体が、略前後方向を軸とする回転（ロール）方向に揺動することから、ロール方向の釣合いを制御することにより、転倒することなく移動又は停止するよう動作を制御する必要があった。ロール方向の転倒を防止する場合、ロール方向の傾斜を正確に検出する必要がある。ロール方向の釣合いを制御するために本体の傾斜を検出する方法として、角速度センサを用いて角速度を検出し、検出した値を積分することによって傾斜を推定する方法、ウエイト（錘）を用いて傾斜を検出する方法等がある。

しかし、角速度センサを用いて角速度を積分する方法では、角速度センサによって検出された角速度出力にノイズ、オフセットが含まれる場合、積分することによってノイズ、オフセット等が増幅される。増幅されたノイズ、オフセット等は累積していくので、目標値からのずれが増大して傾斜角推定及び転倒防止制御を継続することができなくなるという問題があった。

一方、ウエイトを用いて傾斜を検出する方法では、重力の影響を受けて瞬時に傾斜を反映することができないことから応答性が悪く、また鉛直方向からの傾斜を検出するので、釣合い状態からの傾斜を検出することができないという問題があった。例えば重心位置がロール方向にずれている場合、横風を受けている場合等には、鉛直方向が釣合い状態とは限らない。転倒しないようにするためには、傾いた状態を釣合い状態に戻すことが必要であり、釣合い状態からの傾斜を検出する必要がある。

例えば特許文献１では、本体の略前後方向に検出軸を向けた角速度センサと、本体の略前後方向に回転軸を向けたモータと、モータの回転位置又は回転速度を検出する回転センサと、モータの回転軸に連結された慣性ロータとを備え、角速度センサの角速度出力とモータに与えるトルク指令とから、釣合い状態に対する本体の傾斜角を推定し、推定した傾斜角を用いて本体の傾斜を補正する転倒防止制御装置が提案されている。

特許文献１では、角速度出力を積分する必要がないのでノイズ、オフセット等が累積することがなく、また釣合い状態に対する傾斜角を推定することができるので、推定した傾斜角を用いて左右方向の釣合い状態に補正することができる。しかし、二輪車等では、左右方向の釣合いを制御することができれば転倒を防止することができるが、一輪車等では、左右方向の釣合いを制御しても前後方向の釣合いを制御しなければ前後方向に転倒することになる。

一輪車の前後方向の傾斜を推定するため、非特許文献１では、ジャイロセンサと２個の加速度センサとを用いている。加速度センサが回転軸から離れるほど加速度センサに振動が加わるので、２個の加速度センサを用いることにより、振動による加速度を消去して正しい値を推定している。２個の加速度センサから算出された角度θａと、ジャイロセンサで得られた角速度の積分値の角度θｚとを出力し、θａとθｚとに含まれる高周波ノイズを除去するためにフィルタをかけ、フィルタリングされた値の偏差ｅを入力してオブザーバゲインをかけた出力をｄθｚとし、ｄθｚで角度θｚを修正した推定値θｚハットを制御に用いている。

非特許文献１では、ジャイロセンサで得られた角速度を単純に積分した角度θｚは経時的にずれていくが、角度θｚを修正した推定値θｚハットは一定値を保つことができるので、修正した推定値θｚハットを用いることにより、ジャイロセンサで得られた角速度から算出される角度のオフセットのずれを修正することができる。
国際公開２００７／０６３６６５号 笠井崇史著、「一輪車の姿勢制御」、筑波大学大学院博士課程システム情報工学研究科修士論文、２００５年１月、ｐ．１−３７

しかし、非特許文献１の一輪車では、加速度センサによって重力方向（鉛直方向）に対する傾斜を計測しているが、上述した左右方向と同様、前後方向でも鉛直方向が釣合い状態とは限らない。そのため、鉛直方向が釣合い状態でない場合には、釣合い状態に補正することができないという問題点があった。また、傾斜を推定するため、ジャイロセンサの他２個の加速度センサを必要とするので、転倒防止制御装置の構成の簡素化及び小型化が困難になるという問題点もあった。

本発明は斯かる事情に鑑みてなされたものであり、簡便な構成で、ノイズ、オフセット等が累積することなく、前後方向への移動を確保しながら釣合い状態からのピッチ方向の傾斜を精度良く補正し、ピッチ方向への転倒を防止することができる転倒防止制御装置及びコンピュータプログラムを提供することを目的とする。

上記目的を達成するために第１発明に係る転倒防止制御装置は、前後方向に回転して移動する車輪と、該車輪の回転軸に連結されて該車輪の上方でピッチ方向及びロール方向に揺動する本体とで構成され、該本体に、ピッチ方向の傾斜角度の角速度であるピッチ角速度を検出するピッチ用角速度センサと、前記車輪の回転と連動し、該車輪を回転させるピッチ用モータと、該ピッチ用モータの回転位置又は回転速度を検出するピッチ用回転センサとを備え、前記車輪の回転に伴う反動トルクを利用して前記本体のピッチ方向の傾斜を補正する転倒防止制御装置であって、前記車輪の前進又は後退指示を受け付ける前進／後退指示受付部と、受け付けた前進又は後退指示及び前記ピッチ用回転センサで検出された回転角度から導出したピッチ方向の回転速度偏差に基づいて目標とするピッチ方向の傾斜角度である目標ピッチ角を算出する目標ピッチ角計算部と、前記ピッチ用角速度センサで検出されたピッチ角速度と、前記目標ピッチ角に基づいて生成される前記ピッチ用モータに与えるピッチ用トルク指令とから、釣合い状態に対して前記本体がピッチ方向に傾斜している角度であるピッチ傾斜角を推定するピッチ傾斜角推定部と、前記目標ピッチ角と前記ピッチ傾斜角とに基づいて、前記ピッチ用トルク指令を生成するピッチ用トルク指令生成部とを備えることを特徴とする。

また、第２発明に係る転倒防止制御装置は、第１発明において、前記前進／後退指示受付部は、前記車輪の前進又は後退指示を回転速度又は回転角度として受け付けることを特徴とする。

また、第３発明に係る転倒防止制御装置は、第１又は第２発明において、前記目標ピッチ角から前記ピッチ傾斜角を減じたピッチ角度偏差から、目標とするピッチ方向の傾斜角度の角速度である目標ピッチ角速度を算出する目標ピッチ角速度計算部を備え、前記ピッチ用トルク指令生成部は、前記目標ピッチ角速度と検出された前記ピッチ角速度との偏差に基づいて、前記ピッチ用トルク指令を生成することを特徴とする。

また、第４発明に係る転倒防止制御装置は、第１乃至第３発明のいずれか１つにおいて、前記ピッチ傾斜角に基づいて、前記本体をピッチ方向に傾斜させるピッチ方向外部トルクを推定するピッチ方向外部トルク推定部と、推定された前記ピッチ方向外部トルクが相殺される方向に前記ピッチ用トルク指令を補正するトルク補正部とを備えることを特徴とする。

また、第５発明に係る転倒防止制御装置は、第１乃至第４発明のいずれか１つにおいて、前記本体に、ロール方向の傾斜角度の角速度であるロール角速度を検出するロール用角速度センサと、ロール方向に回転する慣性ロータと、該慣性ロータの回転と連動し、該慣性ロータを回転させるロール用モータと、該ロール用モータの回転位置又は回転速度を検出するロール用回転センサとを備え、前記ロール用回転センサで検出されたロール方向の回転速度に基づいて目標とするロール方向の傾斜角度である目標ロール角を算出する目標ロール角計算部と、前記ロール用角速度センサで検出されたロール角速度と、前記目標ロール角に基づいて生成される前記ロール用モータに与えるロール用トルク指令とから、釣合い状態に対して前記本体がロール方向に傾斜している角度であるロール傾斜角を推定するロール傾斜角推定部と、前記目標ロール角から前記ロール傾斜角を減じたロール角度偏差から、目標とするロール方向の傾斜角度の角速度である目標ロール角速度を算出する目標ロール角速度計算部と、前記目標ロール角速度と検出された前記ロール角速度との偏差に基づいて、前記ロール用トルク指令を生成するロール用トルク指令生成部とを備えることを特徴とする。

また、第６発明に係る転倒防止制御装置は、第５発明において、前記ロール用角速度センサの検出軸は、前記本体が前記車輪の回転軸と交差する鉛直線上に重心を有する状態で、前後方向かつ水平方向を向いていることを特徴とする。

また、第７発明に係る転倒防止制御装置は、第１乃至第６発明のいずれか１つにおいて、前記車輪の回転は、前記ピッチ用モータの回転に対して回転比が低くなっていることを特徴とする。

次に、上記目的を達成するために第８発明に係るコンピュータプログラムは、前後方向に回転して移動する車輪と、該車輪の回転軸に連結されて該車輪の上方でピッチ方向及びロール方向に揺動する本体とで構成され、該本体に、ピッチ方向の傾斜角度の角速度であるピッチ角速度を検出するピッチ用角速度センサと、前記車輪の回転と連動し、該車輪を回転させるピッチ用モータと、該ピッチ用モータの回転位置又は回転速度を検出するピッチ用回転センサとを備え、前記車輪の回転に伴う反動トルクを利用して前記本体のピッチ方向の傾斜を補正する転倒防止制御装置に搭載されているコンピュータで実行することが可能なコンピュータプログラムであって、前記コンピュータを、前記車輪の前進又は後退指示を受け付ける前進／後退指示受付手段、受け付けた前進又は後退指示及び前記ピッチ用回転センサで検出された回転角度から導出したピッチ方向の回転速度偏差に基づいて目標とするピッチ方向の傾斜角度である目標ピッチ角を算出する目標ピッチ角計算手段、前記ピッチ用角速度センサで検出されたピッチ角速度と、前記目標ピッチ角に基づいて生成される前記ピッチ用モータに与えるピッチ用トルク指令とから、釣合い状態に対して前記本体がピッチ方向に傾斜している角度であるピッチ傾斜角を推定するピッチ傾斜角推定手段、及び前記目標ピッチ角と前記ピッチ傾斜角とに基づいて、前記ピッチ用トルク指令を生成するピッチ用トルク指令生成手段として機能させることを特徴とする。

また、第９発明に係るコンピュータプログラムは、第８発明において、前記前進／後退指示受付手段を、前記車輪の前進又は後退指示を回転速度又は回転角度として受け付ける手段として機能させることを特徴とする。

また、第１０発明に係るコンピュータプログラムは、第８又は第９発明において、前記コンピュータを、前記目標ピッチ角から前記ピッチ傾斜角を減じたピッチ角度偏差から、目標とするピッチ方向の傾斜角度の角速度である目標ピッチ角速度を算出する目標ピッチ角速度計算手段として機能させ、前記ピッチ用トルク指令生成手段を、前記目標ピッチ角速度と検出された前記ピッチ角速度との偏差に基づいて、前記ピッチ用トルク指令を生成する手段として機能させることを特徴とする。

また、第１１発明に係るコンピュータプログラムは、第８乃至第１０発明のいずれか１つにおいて、前記コンピュータを、前記ピッチ傾斜角に基づいて、前記本体をピッチ方向に傾斜させるピッチ方向外部トルクを推定するピッチ方向外部トルク推定手段、及び推定された前記ピッチ方向外部トルクが相殺される方向に前記ピッチ用トルク指令を補正するトルク補正手段として機能させることを特徴とする。

また、第１２発明に係るコンピュータプログラムは、第８乃至第１１発明のいずれか１つにおいて、前記本体に、ロール方向の傾斜角度の角速度であるロール角速度を検出するロール用角速度センサと、ロール方向に回転する慣性ロータと、該慣性ロータの回転と連動し、該慣性ロータを回転させるロール用モータと、該ロール用モータの回転位置又は回転速度を検出するロール用回転センサとを備え、前記コンピュータを、前記ロール用回転センサで検出されたロール方向の回転速度に基づいて目標とするロール方向の傾斜角度である目標ロール角を算出する目標ロール角計算手段、前記ロール用角速度センサで検出されたロール角速度と、前記目標ロール角に基づいて生成される前記ロール用モータに与えるロール用トルク指令とから、釣合い状態に対して前記本体がロール方向に傾斜している角度であるロール傾斜角を推定するロール傾斜角推定手段、前記目標ロール角から前記ロール傾斜角を減じたロール角度偏差から、目標とするロール方向の傾斜角度の角速度である目標ロール角速度を算出する目標ロール角速度計算手段、及び前記目標ロール角速度と検出された前記ロール角速度との偏差に基づいて、前記ロール用トルク指令を生成するロール用トルク指令生成手段として機能させることを特徴とする。

第１発明及び第８発明では、車輪の前進又は後退指示を受け付け、受け付けた前進又は後退指示及びピッチ用回転センサで検出された回転角度から導出したピッチ方向の回転速度偏差に基づいて、目標とするピッチ方向の傾斜角度である目標ピッチ角を算出する。ピッチ用角速度センサで検出されたピッチ角速度と、目標ピッチ角に基づいて生成されるピッチ用モータに与えるピッチ用トルク指令とから、釣合い状態に対して本体がピッチ方向に傾斜している角度であるピッチ傾斜角を推定する。目標ピッチ角とピッチ傾斜角とに基づいて、ピッチ用トルク指令を生成する。これにより、釣合い状態に対して本体がピッチ方向に傾斜しているピッチ傾斜角を精度良く推定することができるとともに、ピッチ用角速度センサによるピッチ角速度を積分することがないので、ノイズ、オフセット等の累積による目標ピッチ角の計算誤差が生じることがなく、車輪の回転に伴う反動トルクを利用し、釣合い状態からのピッチ方向の傾斜を精度良く補正して、ピッチ方向への転倒を防止することができる。

ここで、「ピッチ傾斜角」とは、ピッチ方向の釣合い状態を基本位置とした場合の、基本位置での本体とピッチ方向に傾斜した本体との偏差角である。「釣合い状態」とは、ピッチ方向での本体等に負荷される重力によって発生するトルク、向かい風、追い風等によって発生するトルク等の外部トルクの合計が０（ゼロ）になる状態である。

第２発明及び第９発明では、車輪の前進又は後退指示を回転速度又は回転角度として受け付けることにより、受け付けた車輪の前進又は後退指示及びピッチ用回転センサで検出された回転角度から導出したピッチ方向の回転速度偏差に基づいて、目標ピッチ角を算出するので、指示された移動のための回転速度を確保しつつ、ピッチ方向の傾斜を補正することができる。さらに、加速度センサ等を別個に必要とせず、車輪の駆動に必要なモータ等の他は、角速度センサを加えただけの簡便な構成で、転倒防止制御装置を簡素化及び小型化することが可能である。

第３発明及び第１０発明では、目標ピッチ角からピッチ傾斜角を減じたピッチ角度偏差から、目標とするピッチ方向の傾斜角度の角速度である目標ピッチ角速度を算出し、目標ピッチ角速度と検出されたピッチ角速度との偏差に基づいて、ピッチ用トルク指令を生成することにより、誤差の累積を抑制しつつ、本体のピッチ方向の傾斜を適正に補正するピッチ用トルク指令を生成することができる。

第４発明及び第１１発明では、ピッチ傾斜角に基づいて、本体をピッチ方向に傾斜させるピッチ方向外部トルクを推定し、トルク補正部で推定されたピッチ方向外部トルクが相殺される方向にピッチ用トルク指令を補正することにより、ピッチ方向外部トルクによる影響を加味してピッチ用モータの回転をより適正に制御することができるので、釣合い状態からのピッチ方向の傾斜をより精度良く補正してピッチ方向への転倒を防止することができる。

ここで、「ピッチ方向外部トルク」とは、釣合い状態からピッチ方向に本体が傾くことによって本体等に負荷される重力によって発生するトルクと、向かい風、追い風等によって本体が傾く方向（ピッチ方向）のトルクとを合わせたトルクである。また、「トルク補正部」とは、ピッチ方向外部トルクが相殺される方向にピッチ用トルク指令を補正する機能を有しており、後述するピッチ用モータトルク指令電圧計算部５０はその一部である。

第５発明及び第１２発明では、ロール用回転センサで検出されたロール方向の回転速度に基づいて目標とするロール方向の傾斜角度である目標ロール角を算出する。ロール用角速度センサで検出されたロール角速度と、目標ロール角に基づいて生成されるロール用モータに与えるロール用トルク指令とから、釣合い状態に対して本体がロール方向に傾斜している角度であるロール傾斜角を推定する。目標ロール角からロール傾斜角を減じたロール角度偏差から、目標とするロール方向の傾斜角度の角速度である目標ロール角速度を算出し、目標ロール角速度と検出されたロール角速度との偏差に基づいて、ロール用トルク指令を生成する。これにより、釣合い状態に対して本体がロール方向に傾斜しているロール傾斜角を精度良く推定することができるとともに、ロール用角速度センサによるロール角速度を積分することがないので、ノイズ、オフセット等の累積による目標ロール角の計算誤差が生じることがなく、慣性ロータの回転に伴う反動トルクを利用し、釣合い状態からのロール方向の傾斜を精度良く補正して、ロール方向への転倒を防止することができる。

第６発明では、ロール用角速度センサの検出軸は、車輪の回転軸と交差する鉛直線上に本体の重心を有する状態で、前後方向かつ水平方向を向くようにすることにより、ロール用角速度センサはロール方向の正確な角速度を検出することができるので、ロール方向の傾斜を精度良く補正することができる。振動子を使用したロール用角速度センサは、振動子の振動方向と直交する方向の力に応じた信号を検出することによって角速度を求める。ロール用角速度センサの検出軸は、略前後方向に向けて本体に取り付けられるので、ロール用角速度センサが検出する力の方向は、略前後方向の検出軸と直交する略鉛直方向となる。ロール用角速度センサの検出軸が、前後方向かつ水平方向を向く状態で角速度を検出する場合、鉛直方向の力を検出することができるので、ロール方向に実際に生じている角速度ωを正確に求めることができる。しかし、本体の重心に偏りがあり、ロール用角速度センサの検出軸が、例えば水平方向より前側が低く前傾した状態で角速度を検出する場合には、鉛直方向から所定角度θだけずれた方向の力を検出して角速度ωｃｏｓθが求まるので、実際の角速度ωより遅い角速度を検出することになる。

第７発明では、車輪の回転は、ピッチ用モータの回転に対して回転比が低くなるように、例えば、ピッチ用モータからかさ歯車、プーリ、ベルトを介して車輪を駆動し、ピッチ用モータ側のプーリを車輪側のプーリに対して減速比を３倍高くすることにより、ピッチ用モータによるトルクの１／３のトルクで車輪を制御することができる。またピッチ用モータによるトルクに対して低いトルク比で車輪を制御することができることにより、実質的なモータの限界トルクが高くなり、転倒を防止することができる傾斜角の許容角を大きくすることが可能となる。

上記構成によれば、本体が釣合い状態に対してピッチ方向に傾斜しているピッチ傾斜角を精度良く推定することができるとともに、ピッチ用角速度センサによるピッチ角速度を積分することがないので、ノイズ、オフセット等の累積による目標ピッチ角の計算誤差が生じることがなく、車輪の回転に伴う反動トルクを利用し、釣合い状態からのピッチ方向の傾斜を精度良く補正して、ピッチ方向への転倒を防止することができる。また、車輪の前進又は後退指示を受け付け、受け付けた指示及びピッチ用回転センサで検出された回転角度から導出したピッチ方向の回転速度偏差に基づいて、目標ピッチ角を算出することにより、指示された移動のための回転速度を確保しつつ、ピッチ方向の傾斜を補正することができる。さらに、加速度センサ等を別個に必要とせず、車輪の駆動に必要なモータ等の他は、角速度センサを加えただけの簡便な構成で、転倒防止制御装置を簡素化及び小型化することが可能である。

本発明の実施の形態に係る転倒防止制御装置を適用した一輪車ロボットの構成を模式的に示す正面図及び側面図である。 ピッチ方向、ロール方向及びヨー方向を説明する模式図である。 一輪車ロボットのピッチ方向の転倒を防止する制御の一例を示す制御ブロック図である。 一輪車ロボットのモデルを側面から見た模式図である。 ピッチ用モータからかさ歯車、プーリ、ベルトを介して車輪を駆動する構成を示す模式図である。 ピッチ用モータからギアを介して直接車輪を駆動する構成の一例を示す模式図である。 ピッチ用モータからギアを介して直接車輪を駆動する構成の他の例を示す模式図である。 ピッチ用モータからかさ歯車、ギアを介して車輪を駆動する構成を示す模式図である。 図５に示す駆動構成を採用する現実的な一輪車ロボットの例示図である。 一輪車ロボットのロール方向の転倒を防止する制御の一例を示す制御ブロック図である。 一輪車ロボットを正面から見た模式図である。 本体の重心位置によるロール用ジャイロセンサで検出される角速度の感度を説明する模式図である。 一輪車ロボットの制御基板のコントローラによるピッチ方向の転倒防止処理手順を示すフローチャートである。 一輪車ロボットの制御基板のコントローラによるロール方向の転倒防止処理手順を示すフローチャートである。

符号の説明

１ 一輪車ロボット（転倒防止制御装置）
２ 車輪
３ 本体
４ フレーム
５ かさ歯車
６ ベルト
３１ ピッチ用ジャイロセンサ（ピッチ用角速度センサ）
３２ ピッチ用モータ
３３ ピッチ用エンコーダ（ピッチ用回転センサ）
３５ 制御基板
３６ 電池
４１ ピッチ用カウンタ部
４２ 前進／後退指示受付部
４３ ピッチ用回転速度計算部
４４ 目標ピッチ角計算部
４５ ピッチ用ＡＤコンバータ部
４６ ピッチ角速度計算部
４７ ピッチ傾斜角推定部
４８ 目標ピッチ角速度計算部
４９ ピッチ用トルク指令生成部
５０ ピッチ用モータトルク指令電圧計算部
５１ ピッチ用ＤＡコンバータ部
５２ ピッチ方向外部トルク推定部
Ｏ、Ｏ₁ 、Ｏ₂ 回転中心
ｍ₁ 本体質量
ｍ₂ 慣性ロータ質量
Ｉ_1p Ｏまわりの本体の慣性モーメント
Ｉ_2p Ｏまわりの車輪の慣性モーメント
Ｉ_1r Ｏ₁ まわりの本体の慣性モーメント
Ｉ_2r Ｏ₂ まわりの慣性ロータの慣性モーメント
θ_1p 垂直軸に対する本体のピッチ方向の傾斜角度
θ_2p 本体に対する車輪の回転角度
θ_1r 垂直軸に対する本体のロール方向の傾斜角度
θ_2r 本体に対する慣性ロータの回転角度
τ_1p 本体に働くＯまわりの外乱トルク
τ_2p 車輪に働くトルク
ｌ_Gp Ｏから本体重心位置までの距離
ｌ_r Ｏ₁ からＯ₂ までの距離
ｌ_Gr Ｏ₁ から本体重心位置までの距離
ｒ 車輪の半径
ｇ 重力加速度

以下、本発明の実施の形態に係る転倒防止制御装置について、一輪車の上方に搭載してある本体が揺動しつつ一輪車の回転により転倒することなく前後に移動する一輪車ロボットに適用した例を図面に基づいて具体的に説明する。

図１は、本発明の実施の形態に係る転倒防止制御装置を適用した一輪車ロボットの構成を模式的に示す正面図及び側面図である。図１（ａ）は正面図、図１（ｂ）は右側面図を示している。本実施の形態に係る転倒防止制御装置は、一輪車ロボット本体のピッチ方向及びロール方向への転倒を防止するよう機能する。

図１（ａ）、（ｂ）に示すように、一輪車ロボット１は、前後方向に回転して移動する車輪２と、車輪２の回転軸に連結されて車輪２の上方でピッチ方向及びロール方向に揺動する本体３とで構成されている。図１（ａ）、（ｂ）の例では、本体３を人型ロボットとしているが、これに限定されるものではない。

ここで、ピッチ方向及びロール方向を明確にしておく。図２は、ピッチ方向、ロール方向及びヨー方向を説明する模式図である。図２に示すように、一輪車ロボット１がｘｙ面上をｘ軸の（＋）方向へ前進又はｘ軸の（−）方向へ後退するように移動する場合、ｙ軸周りの回転方向がピッチ方向である。ｙ軸の（＋）方向を向いて反時計回りに回転した場合には本体３が前方へ傾斜し、ｙ軸の（＋）方向を向いて時計回りに回転した場合には本体３が後方へ傾斜する。また、ｘ軸周りの回転方向がロール方向であり、本体３が左右方向へ揺動する場合の回転方向である。さらに、ｚ軸周りの回転方向がヨー方向であり、車輪２の向きをｘ軸方向から傾ける場合の回転方向である。

図１（ａ）、（ｂ）に示すように、本体３には、ピッチ方向の傾斜角度の角速度であるピッチ角速度を検出するピッチ用ジャイロセンサ（ピッチ用角速度センサ）３１、車輪２の回転と連動し、車輪２を回転させるピッチ用モータ３２、及びピッチ用モータ３２の回転位置又は回転速度を検出するピッチ用エンコーダ（ピッチ用回転センサ）３３を備える。ピッチ用ジャイロセンサ３１は、ピッチ角速度を検出する図示しない検出軸を略左右方向に向けて本体３に取り付けられている。ここで、略左右方向とは、厳密な左右方向に対して上下に若干の角度ずれがあってもよいことを意味する。本体３と車輪２とは、車輪２を回転自在に支持するフレーム４で連結され、ピッチ用モータ３２による回転は、本体３に備えたかさ歯車５とベルト６とを介して車輪２へ伝わる。フレーム４は、本体３の一部分であり、図１（ａ）、（ｂ）の例では、フレーム４は本体３である人型ロボットの足になっている。なお、ピッチ用角速度センサは、ピッチ角速度を検出することができれば良く、ジャイロセンサに限定されるものではない。

また、本体３である人型ロボットの背中には、ピッチ用モータ３２を制御する制御基板３５及び電池３６が装備されている。制御基板３５には、ピッチ用モータ３２を回転駆動するドライバ、Ａ／Ｄコンバータ、Ｄ／Ａコンバータ、カウンタ、コントローラ等が搭載されている。コントローラは、具体的にはマイクロプロセッサ、ＣＰＵ、ＬＳＩ等である。一輪車ロボット１は、ピッチ方向の釣合いをとるため、車輪２を駆動するときの反動を利用して釣合いをとるように制御している。図３は、一輪車ロボット１のピッチ方向の転倒を防止する制御の一例を示す制御ブロック図である。

図３に示すように、ピッチ用カウンタ部４１では、ピッチ用エンコーダ３３の出力パルスが計数される。前進／後退指示受付部４２では、車輪２の前進指示又は後退指示を回転速度又は回転角度として受け付ける。前進指示又は後退指示が回転角度として出力された場合、ピッチ用回転速度計算部４３では、前進指示又は後退指示のパルス信号のパルス数からピッチ用カウンタ部４１で計数されたパルス数を減算し、減算して得たパルス数を回転角度に変換した後、微分してピッチ用モータ３２の回転速度を求める。ノイズ除去のためのＬＰＦ（ローパスフィルタ）を装備しても良い。

目標ピッチ角計算部４４では、ピッチ用回転速度計算部４３で求めたピッチ用モータ３２の回転速度から、ピッチ用モータ３２の回転が、車輪２が前進する方向に回転する場合には、目標ピッチ角を車輪２が前進する方向に、ピッチ用モータ３２の回転が、車輪２が後退する方向に回転する場合には、目標ピッチ角を車輪２が後退する方向になるように、ピッチ用モータ３２の回転速度に比例係数を乗算して求める。これにより、指示された移動のための回転速度を確保しつつ、ピッチ方向の傾斜を補正することができる。

一方、ピッチ用ＡＤコンバータ部４５では、ピッチ用ジャイロセンサ３１のピッチ角速度出力を測定する。ピッチ角速度計算部４６では、ピッチ角速度出力に変換係数を乗算して、ピッチ角速度ω_1pを算出する。

ピッチ傾斜角推定部４７では、ピッチ角速度ω_1pと、後述のピッチ用トルク指令τ_2pとから、本体３と車輪２とを含んだ系の傾斜角方向（ピッチ方向）の運動方程式に基づき導出された後述の（式１８）で、ピッチ傾斜角を算出する。さらに適切な推定速度を持たせてループを安定化させるための一次遅れ要素を直列に加えることで、ピッチ傾斜角の推定値を算出している。具体的には、（式１８）を用いた計算値に、一次遅れ要素として例えば１／（０．１Ｓ＋１）を直列に加えることになるが、これらに限られるものではなく、適切な推定速度となるような任意の遅れ要素を加えることができる。

ピッチ方向外部トルク推定部５２では、ピッチ傾斜角の推定値に変換係数を乗算して、本体３に働いているピッチ方向外部トルクの推定値を算出し、ピッチ用補正トルク（ピッチ方向外部トルクの推定値に相当）τ_3pを生成する。

目標ピッチ角速度計算部４８では、目標ピッチ角からピッチ傾斜角の推定値を減じたピッチ角度偏差に比例ゲインを乗算して、目標ピッチ角速度ω_2pを算出する。ピッチ用トルク指令生成部４９では、目標ピッチ角速度ω_2pとピッチ角速度ω_1pとの偏差に対して、例えばＰＩ制御によりピッチ用トルク指令τ_0pを生成する。ピッチ用モータトルク指令電圧計算部５０では、ピッチ用トルク指令τ_0pとピッチ用補正トルクτ_3pとを加えたピッチ用トルク指令τ_2pに対して、変換係数を乗算して、指令電圧を生成する。最後に、ピッチ用ＤＡコンバータ部５１では、ドライバに指令電圧を出力し、ピッチ用モータ３２の回転を制御する。

ここで、（式１８）で表わされるピッチ傾斜角の計算式の導出方法について、以下に説明する。図４は、一輪車ロボット１のモデルを側面から見た模式図である。図４では、車輪２、本体３及び本体３に取り付けられたピッチ用ジャイロセンサ３１のみを模式的に示し、矢印方向が前進方向であり、本体３が前傾した状態を示している。まずラグランジュ方程式により、運動方程式を導出する。本体３と車輪２とを合わせた、全体の運動エネルギーＴ及び位置エネルギーＵは以下のようになる。

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

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

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

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

（式１３）を（式１１）に代入してｓｉｎθ_1pをθ_1pで近似すると、（式１４）を得る。（式１４）より、本体３の運動は、車輪２の角度及び角速度には無関係となる。

−ピッチ傾斜角の推定−
ピッチ傾斜角は、ピッチ用ジャイロセンサ３１の出力の積分で求めることも可能であるが、偏差が累積して不正確になるため、別の方法で求める必要がある。そこで、図４に示すモデルの運動方程式を用いて、ピッチ用ジャイロセンサ３１が出力するピッチ角速度ω_1pと、ピッチ用トルク指令τ_2pとから、ピッチ傾斜角を推定する。運動方程式（式１４）を変形すると、（式１５）となる。

一方、ピッチ用ジャイロセンサ３１が出力するピッチ角速度ω_1pは、（式１６）で表わされる。

また、向かい風、追い風等によって本体３が傾く方向（ピッチ方向）のトルクτ_1pが発生した場合、見かけの釣合い傾斜角θ_0pは（式１７）となる。

従って、見かけの釣合い傾斜角に対する現在のピッチ方向の傾斜角θ_1pの偏差角（ピッチ傾斜角）は、上記（式１５）、（式１６）、（式１７）より、（式１８）を導出して算出することによって推定することができる。ただし、適切な推定速度を持たせてループを安定化させるために、一次遅れ要素を直列に加えておくのが良い。なお、（式１８）はピッチ傾斜角を推定する計算式の一例であり、対象モデルによってピッチ傾斜角を推定する計算式は異なる場合がある。

ピッチ用ジャイロセンサ３１で出力されたピッチ角速度ω_1pと、目標ピッチ角に基づいて生成されるピッチ用モータ３２に与えるピッチ用トルク指令τ_2pとから、釣合い状態に対して本体３がピッチ方向へ傾斜している角度であるピッチ傾斜角を推定することにより、ピッチ傾斜角を精度良く推定することができる。また、ピッチ用ジャイロセンサ３１によるピッチ角速度を積分することがないので、ノイズ、オフセット等の累積による目標ピッチ角の計算誤差が生じることがなく、車輪２の回転に伴う反動トルクを利用し、釣合い状態からのピッチ方向の傾斜を精度良く補正して、ピッチ方向への転倒を防止することができる。

−ピッチ方向外部トルクフィードフォワード−
(式１８)で推定された偏差角（ピッチ傾斜角）によって、ピッチ方向外部トルクを補償する。

（式１９）として、トルクに加えておく。

（式２０）とすれば、運動方程式（式１４）は、（式２１）となるから、ピッチ方向外部トルクを補償することができる。釣合い状態から本体３がピッチ方向へ傾斜している角度であるピッチ傾斜角を推定した（式１８）によって、釣合い状態からのピッチ方向への傾斜角度によって生じるピッチ方向外部トルクを推定することができるので、推定したピッチ方向外部トルクを相殺する補正トルクを算出することができる。したがって、ピッチ方向外部トルクによる影響を加味してピッチ用モータ３２の回転をより適正に制御することができるので、釣合い状態からのピッチ方向の傾斜をより精度良く補正してピッチ方向への転倒を防止することができる。特に、傾斜角ループ、傾斜角速度ループの応答周波数が低い場合でも、ピッチ方向外部トルクをフィードフォワード制御で補償することにより、転倒防止制御を継続することができるため、安定な制御が可能となる。

補正されたピッチ用トルク指令は、ピッチ用ＤＡコンバータ部５１を介してドライバへ出力され、ピッチ用モータ３２の回転が制御される。ピッチ用モータ３２による回転は、車輪２へ伝わる。図５は、ピッチ用モータ３２からかさ歯車５、プーリ、ベルト６を介して車輪２を駆動する構成を示す模式図である。図５（ａ）は正面図、図５（ｂ）は右側面図を示している。図５は、一輪車ロボット１において車輪２の駆動にかかわる車輪２及び本体３の車輪２側部分のみを示している。

図５（ａ）、（ｂ）に示すように、ピッチ用モータ３２による回転は、順にモータ軸３２１、かさ歯車５、５、本体３側のプーリ７、ベルト６、車輪２側のプーリ８を介して車輪２へ伝わる。一輪車ロボット１では、車輪２の回転は、ピッチ用モータ３２の回転に対して回転比が低くなるようにしている。具体的には、ピッチ用モータ３２側、すなわち本体３側のプーリ７を車輪２側のプーリ８に対して減速比を高くしている。

例えば減速比を３倍高くすることにより、ピッチ用モータ３２によるトルク、すなわち上述した補正されたピッチ用トルク指令によって発生するトルクの１／３のトルクで車輪２を制御することができる。したがって、実質的なピッチ用モータ３２の限界トルクを高くすることができ、転倒を防止することができる傾斜角の許容角を大きくすることが可能となる。

図６は、ピッチ用モータ３２からギアを介して直接車輪２を駆動する構成の一例を示す模式図である。図６（ａ）は正面図、図６（ｂ）は右側面図を示している。図６（ａ）、（ｂ）に示すように、ピッチ用モータ３２による回転は、ピッチ用モータ３２のモータ軸３２１のギア３２２と噛み合うギア３２３と同軸につれまわり回転するギア３２４によって、直接車輪２のタイヤ３２５に伝わる。

図７は、ピッチ用モータ３２からギアを介して直接車輪２を駆動する構成の他の例を示す模式図である。図７（ａ）は正面図、図７（ｂ）は右側面図を示している。なお、図７（ａ）の正面図では、説明のため車輪２を断面で示している。図７（ａ）、（ｂ）に示すように、図６の構成と同様、ピッチ用モータ３２による回転は、ピッチ用モータ３２のモータ軸３２１のギア３２２と噛み合うギア３２３と同軸につれまわり回転するギア３２４によって、直接車輪２に伝わるが、図６の構成のように車輪２のタイヤ３２５に伝わるのではなく、車輪２のホイール３２６に形成された歯車に伝わる点で図６の構成と異なる。

一輪車ロボット１では、図６又は図７に示す駆動構成を採用しても良い。ただし、図６に示す駆動構成では、ギア３２４を直接車輪２のタイヤに押し当てて回転を伝えるため、すべり等が発生すると、精度良く車輪２の回転を制御することは困難となる。

図８は、ピッチ用モータ３２からかさ歯車５、５、ギア９を介して車輪２を駆動する構成を示す模式図である。図８（ａ）は正面図、図８（ｂ）は右側面図を示している。図８（ａ）、（ｂ）に示すように、ピッチ用モータ３２による回転は、ピッチ用モータ３２のモータ軸３２１を介してかさ歯車５から車輪２の回転軸とつれまわり回転するギア９によって、車輪２に伝わる。図８に示す駆動構成では、図５に示す駆動構成と同様、歯車５又はギア９のかみ合わせのいわゆるガタ分の揺り戻し（バックラッシュ）が発生する程度で、図６及び図７に示す駆動構成のようなすべり等は発生しないので、高精度に車輪２の回転を制御することができる。また図８に示す駆動構成は、かさ歯車５とギア９との間にさらにギアを介在させてギア比（減速比）を変更することにより、図５に示す駆動構成のように実質的なピッチ用モータ３２の限界トルクを高くすることも可能である。

図９は、図５に示す駆動構成を採用する現実的な一輪車ロボット１の例示図である。図９（ａ）は正面図、図９（ｂ）は右側面図を示している。図９（ａ）、（ｂ）に示す駆動構成は図５と同じであるので、構成の説明は省略する。図９に示すように、かさ歯車５、プーリ７、８及びベルト６をフレーム４内に納めて、側面から見えないようにすることができ、図８に示す駆動構成のようにギア９等が見えてしまう構成と比較して意匠的な効果も得られる。したがって、一輪車ロボット１では、図５乃至図８に示すいずれの駆動構成を採用しても良いが、上述した種々の効果を得られる図５に示す駆動構成を採用することが特に好ましい。

一輪車ロボット１は、上述のようにピッチ方向及びロール方向への転倒を防止するよう機能するので、以降、ロール方向への転倒を防止する構成について説明する。

図１（a）、（ｂ）に示すように、上述したピッチ方向への転倒を防止する構成に加え、本体３には、ロール方向の傾斜角度の角速度であるロール角速度を検出するロール用ジャイロセンサ（ロール用角速度センサ）６１、ロール方向に回転する慣性ロータ６４、慣性ロータ６４の回転と連動し、慣性ロータ６４を回転させるロール用モータ６２、及びロール用モータ６２の回転位置又は回転速度を検出するロール用エンコーダ（ロール用回転センサ）６３を備える。ロール用ジャイロセンサ６１は、ロール角速度を検出する図示しない検出軸を略前後方向に向けて本体３に取り付けられている。ここで、略前後方向とは、厳密な前後方向に対して上下に若干の角度ずれがあってもよいことを意味する。なお、ロール用角速度センサ６１は、ロール角速度を検出することができれば良く、ジャイロセンサに限定されるものではない。

また本体３である人型ロボットの背中に装備されている上述した制御基板３５は、ピッチ用モータ３２を制御するだけでなく、ロール用モータ６２も制御する。図１０は、一輪車ロボット１のロール方向の転倒を防止する制御の一例を示す制御ブロック図である。

図１０に示すように、ロール用カウンタ部７１では、ロール用エンコーダ６３の出力パルスが計数される。ロール用回転速度計算部７３では、ロール用カウンタ部７１の出力を回転角度に変換したあと、微分してロール用モータ６２の回転速度を求める。ノイズ除去のためのＬＰＦ（ローパスフィルタ）を装備しても良い。

目標ロール角計算部７４では、ロール用モータ６２の回転が一輪車ロボット１の正面から見て左回転の場合は、目標ロール角を一輪車ロボット１の正面から見て右方向に、ロール用モータ６２の回転が一輪車ロボット１の正面から見て右回転の場合は、目標ロール角を一輪車ロボット１の正面から見て左方向になるように、ロール用モータ６２の回転速度に比例係数を乗算して求める。なお、積分器を追加して、慣性ロータ６４に定常回転が残留しないようにするのが良い。

一方、ロール用ＡＤコンバータ部７５では、ロール用ジャイロセンサ６１のロール角速度出力を取得する。ロール角速度計算部７６では、ロール角速度出力に変換係数を乗算して、ロール角速度ω_1rを算出する。

ロール傾斜角推定部７７では、ロール角速度ω_1rと、後述のロール用トルク指令τ_2rとから、本体３（慣性ロータ６４以外の部分）と慣性ロータ６４とを含んだ系の傾斜角方向（ロール方向）の運動方程式に基づき導出された、後述の（式２５）で表されるロール傾斜角を算出する。さらに適切な推定速度を持たせてループを安定化させるための一次遅れ要素を直列に加えることで、ロール傾斜角の推定値を算出している。具体的には、（式２５）を用いて算出した計算値に、一次遅れ要素として例えば１／（０．１Ｓ＋１）を直列に加えることになるが、これらに限られるものではなく、適切な推定速度となるような任意の遅れ要素を加えることができる。

ロール方向外部トルク推定部８２では、ロール傾斜角の推定値に変換係数を乗算して、本体３に働いているロール方向外部トルクの推定値を算出し、ロール用補正トルク（ロール方向外部トルクの推定値に相当）τ_3rを生成する。

目標ロール角速度計算部７８では、目標ロール角からロール傾斜角の推定値を減じたロール角度偏差に比例ゲインを乗算して、目標ロール角速度ω_2rを生成する。ロール用トルク指令生成部７９では、目標ロール角速度ω_2rとロール角速度ω_1rとの偏差に対して、例えばＰＩ制御によりロール用トルク指令τ_0rを生成する。ロール用モータトルク指令電圧計算部８０では、ロール用トルク指令τ_0rとロール用補正トルクτ_3rとを加えたロール用トルク指令τ_2rに対して、変換係数を乗算して、指令電圧を生成する。最後に、ロール用ＤＡコンバータ部８１では、ドライバに指令電圧を出力し、ロール用モータ６２の回転を制御する。

ここで、（式２５）で表わされるロール傾斜角の計算式の導出方法について、以下に説明する。図１１は、一輪車ロボット１を正面から見た模式図である。図１１では、本体３及び本体３に取り付けられた慣性ロータ６４のみを模式的に示している。まずラグランジュ方程式により、運動方程式を導出する。本体３（慣性ロータ６４以外の部分）と慣性ロータ６４とを合わせた、全体の運動エネルギーＴ及び位置エネルギーＵは以下のようになる。

上述したピッチ傾斜角を算出する計算式を導出したようにして、（式２２）、（式２３）から、（式２４）が得られる。（式２４）より、本体３の運動は、慣性ロータ６４の角度及び角速度には無関係となる。

−ロール傾斜角の推定−
上述したピッチ傾斜角と同様、ロール傾斜角は、ロール用ジャイロセンサ６１の出力の積分で求めることも可能であるが、偏差が累積して不正確になるため、別の方法で求める必要がある。上述したピッチ傾斜角を推定する場合と同様にして、図１１に示すモデルの運動方程式を用いて、ロール用ジャイロセンサ６１が出力するロール角速度ω_1rと、ロール用トルク指令τ_2rとから、ロール傾斜角を推定する。

横風、カーブ走行による遠心力等によって本体３が傾く方向（ロール方向）のトルクτ_1rが発生した場合、見かけの釣合い傾斜角に対する現在のロール方向の傾斜角θ_1rの偏差（ロール傾斜角）は、運動方程式（式２４）を変形した式と、ロール用ジャイロセンサ６１で出力されたロール角速度ω_1rとから、（式２５）で推定することができる。なお、適切な推定速度を持たせてループを安定化させるために、一次遅れ要素を直列に加えておくのが良い。また（式２５）はロール傾斜角を推定する計算式の一例であり、対象モデルによってロール傾斜角を推定する計算式は異なる場合がある。

ロール用ジャイロセンサ６１で出力されたロール角速度ω_1rと、目標ロール角に基づいて生成されるロール用モータ６２に与えるロール用トルク指令τ_2rとから、釣合い状態に対して本体３がロール方向へ傾斜している角度であるロール傾斜角を推定することにより、ピッチ傾斜角と同様に、ロール傾斜角を精度良く推定することができる。また、ロール角速度センサによるロール角速度を積分することがないので、ノイズ、オフセット等が累積による目標ロール角の計算誤差が生じることがなく、車輪２の回転に伴う反動トルクを利用し、釣合い状態からのロール方向の傾斜を精度良く補正して、ロール方向への転倒を防止することができる。

−ロール方向外部トルクフィードフォワード−
(式２５)で推定された偏差角度によって、ロール方向外部トルクを補償する。

（式２６）として、トルクに加えておく。

（式２７）とすれば、運動方程式（式２４）は、（式２８）となるから、ロール方向外部トルクを補償することができる。したがって、ピッチ方向と同様に、ロール方向外部トルクによる影響を加味してロール用モータ６２の回転をより適正に制御することができるので、釣合い状態からのロール方向の傾斜をより精度良く補正してロール方向への転倒を防止することができる。

図１２は、本体３の重心位置によるロール用ジャイロセンサ６１で検出される角速度の感度を説明する模式図である。図１２は、一輪車ロボット１の右側面から見た図であり、太矢印ａが前進方向を示し、一輪車ロボット１のピッチ方向の釣合い状態を示している。ロール用ジャイロセンサ６１は、振動子を使用した角速度センサであり、振動子の振動方向と直交する方向の力に応じた信号を検出することによって角速度を求める。なお、検出する力の方向は、略前後方向の検出軸と直交する略鉛直方向となる。図１２に示すように、本体３の重心Ｇは本体３の軸３８上にはなく、本体３の後ろ寄りにあるため、一輪車ロボット１は、ピッチ方向の釣合い状態で本体３の軸３８が鉛直方向ではなく、前傾している。ロール用ジャイロセンサ６１は、本体３の軸３８が鉛直方向になる状態で、検出軸が前後方向かつ水平方向を向き、鉛直方向の力に応じた信号を検出するように本体３に取り付けられている。

図１２に示すようなピッチ方向の釣合い状態では、ロール用ジャイロセンサ６１の検出軸は、矢印ｂを向いて水平方向より前側が低く前傾している。検出軸が前傾した状態で、ロール用ジャイロセンサ６１は、矢印ｄで示す力に応じた信号を検出するが、ロール方向に実際に生じている角速度は、矢印ｃで示す力に対応する。矢印ｃで示す力に対応する角速度をωとすると、矢印ｄで示す力に対応する角速度はωｃｏｓθとなり、ロール用ジャイロセンサ６１が検出する角速度は実際の角速度より遅くなる。ロール用ジャイロセンサ６１が実際の角速度を正確に検出することができなければ、ロール傾斜角を精度良く推定することができず、ロール方向の傾斜を精度良く補正することができない。

したがって、ロール方向に実際に生じている角速度を正確に検出するためには、ロール用ジャイロセンサ６１の検出軸が、図１２のようなピッチ方向の釣合い状態、すなわち、本体３が車輪２の軸と交差する鉛直線上に重心Ｇを有する状態で、前後方向かつ水平方向を向くようにして、ロール用ジャイロセンサ６１を本体３に取り付けることが好ましい。これにより、ロール用ジャイロセンサ６１は、ピッチ方向の釣合い状態でロール方向に実際に生じている角速度を正確に検出することができ、ピッチ方向の釣合い状態からピッチ方向に微小角だけ揺動する状態でも、実際のロール角速度と小さい誤差で感度良くロール角速度を検出することができる。

次に、上述した図３に示す制御ブロックで構成される一輪車ロボット１の制御動作について、フロ−チャートに基づいて説明する。説明を簡易にするため、ピッチ方向の転倒防止制御とロール方向の転倒防止制御とを分けて説明する。図１３は、一輪車ロボット１の制御基板３５のコントローラによるピッチ方向の転倒防止処理手順を示すフローチャートである。

図１３に示すように制御基板３５のコントローラは、ピッチ用モータ３２の回転を検知したピッチ用エンコーダ３３の出力（パルス信号）のパルス数を計数する（ステップＳ１２０１）。コントローラは、車輪２の前進（又は後退）指示を回転速度のパルス信号として受け付ける（ステップＳ１２０２）。

コントローラは、前進（又は後退）指示のパルス信号のパルス数から、ピッチ用エンコーダ３３の出力（パルス信号）のパルス数を減算して得たパルス数から、ピッチ方向の回転速度偏差を算出する（ステップＳ１２０３）。具体的には、減算して得たパルス数を回転角度に変換した後、微分して回転速度偏差を求める。コントローラは、ピッチ方向の回転速度偏差に基づいて目標とするピッチ方向の傾斜角度である目標ピッチ角を算出する（ステップＳ１２０４）。

コントローラは、算出した目標ピッチ角から後述するステップＳ１２１２で推定したピッチ傾斜角を減じてピッチ角度偏差を算出し（ステップＳ１２０５）、算出したピッチ角度偏差に比例ゲインを乗算して目標ピッチ角速度ω_2pを算出する（ステップＳ１２０６）。

コントローラは、目標ピッチ角速度ω_2pと、後述するステップＳ１２１１で算出したピッチ角速度ω_1pとのピッチ角速度偏差を算出し（ステップＳ１２０７）、算出したピッチ角速度偏差に対してＰＩ制御等によってピッチ用トルク指令τ_0pを生成する（ステップＳ１２０８）。

コントローラは、生成したピッチ用トルク指令τ_0pを後述するステップＳ１２１３で推定したピッチ方向外部トルクτ_3pで補正し、ピッチ用トルク指令τ_2pを生成する（ステップＳ１２０９）。

コントローラは、ピッチ用ジャイロセンサ３１で出力されたピッチ角速度の出力をＡ／Ｄ変換して取得する（ステップＳ１２１０）。コントローラは、取得したピッチ角速度の出力に変換係数を乗算してピッチ角速度ω_1pを算出する（ステップＳ１２１１）。

コントローラは、（式１８）を用いて、算出したピッチ角速度ω_1pと、上述したステップＳ１２０９で生成したピッチ用トルク指令τ_2pとから、釣合い状態に対して本体３がピッチ方向に傾斜しているピッチ傾斜角を推定する（ステップＳ１２１２）。コントローラは、推定したピッチ傾斜角に基づいて、本体３をピッチ方向に傾斜させるピッチ方向外部トルクを推定する（ステップＳ１２１３）。

コントローラは、ステップＳ１２０９でピッチ用トルク指令τ_2pを生成したか否かを判断する（ステップＳ１２１４）。

コントローラが、ピッチ用トルク指令τ_2pを生成したと判断した場合（ステップＳ１２１４：ＹＥＳ）、コントローラは、生成したピッチ用トルク指令τ_2pに変換係数を乗算して指令電圧を算出する（ステップＳ１２１５）。コントローラは、算出した指令電圧をＤ／Ａ変換し、ピッチ用モータ３２を回転駆動するドライバに出力する（ステップＳ１２１６）。コントローラは、処理をステップＳ１２０１及びステップＳ１２１０に戻し、上述した処理を繰り返す。

一方、コントローラが、ピッチ用トルク指令τ_2pを生成しなかったと判断した場合（ステップＳ１２１４：ＮＯ）、本体３は釣合い状態で前進／後退指示もない状態にあり、コントローラは、処理を終了する。上述した例は、前進指示又は後退指示として回転角度を受け付けた場合の処理手順を示しているが、前進指示又は後退指示として回転速度を受け付けた場合であっても、角速度偏差を求めることにより同様の処理手順によりピッチ方向の転倒防止制御が可能となる。

図１４は、一輪車ロボット１の制御基板３５のコントローラによるロール方向の転倒防止処理手順を示すフローチャートである。図１４に示すように制御基板３５のコントローラは、ロール用モータ６２の回転を検知したロール用エンコーダ６３の出力（パルス信号）のパルス数を計数する（ステップＳ１３０１）。

コントローラは、計数したパルス数からロール方向の回転速度を算出する（ステップＳ１３０２）。具体的には、計数したパルス数を回転角度に変換した後、微分して回転速度を算出する。コントローラは、ロール方向の回転速度に基づいて目標とするロール方向の傾斜角度である目標ロール角を算出する（ステップＳ１３０３）。

コントローラは、算出した目標ロール角から後述するステップＳ１３１１で推定したロール傾斜角を減じてロール角度偏差を算出し（ステップＳ１３０４）、算出したロール角度偏差に比例ゲインを乗算して目標ロール角速度ω_2rを算出する（ステップＳ１３０５）。

コントローラは、目標ロール角速度ω_2rと、後述するステップＳ１３１０で算出したロール角速度ω_1rとのロール角速度偏差を算出し（ステップＳ１３０６）、算出したロール角速度偏差に対してＰＩ制御等によってロール用トルク指令τ_0rを生成する（ステップＳ１３０７）。

コントローラは、生成したロール用トルク指令τ_0rを後述するステップＳ１３１２で推定したロール方向外部トルクτ_3rで補正し、ロール用トルク指令τ_2rを生成する（ステップＳ１３０８）。

コントローラは、ロール用ジャイロセンサ６１で出力されたロール角速度の出力をＡ／Ｄ変換して取得する（ステップＳ１３０９）。コントローラは、取得したロール角速度の出力に変換係数を乗算してロール角速度ω_1rを算出する（ステップＳ１３１０）。

コントローラは、（式２５）を用いて、算出したロール角速度ω_1rと、上述したステップＳ１３０８で生成したロール用トルク指令τ_2rとから、釣合い状態に対して本体３がロール方向に傾斜しているロール傾斜角を推定する（ステップＳ１３１１）。コントローラは、推定したロール傾斜角に基づいて、本体３をロール方向に傾斜させるロール方向外部トルクを推定する（ステップＳ１３１２）。

コントローラは、ステップＳ１３０８でロール用トルク指令τ_2rを生成したか否かを判断する（ステップＳ１３１３）。

コントローラが、ロール用トルク指令τ_2rを生成したと判断した場合（ステップＳ１３１３：ＹＥＳ）、コントローラは、生成したロール用トルク指令τ_2rに変換係数を乗算して指令電圧を算出する（ステップＳ１３１４）。コントローラは、算出した指令電圧をＤ／Ａ変換し、ロール用モータ６２を回転駆動するドライバに出力する（ステップＳ１３１５）。コントローラは、処理をステップＳ１３０１及びステップＳ１３０９に戻し、上述した処理を繰り返す。

一方、コントローラが、ロール用トルク指令τ_2rを生成しなかったと判断した場合（ステップＳ１３１３：ＮＯ）、本体３は釣合い状態であり、コントローラは、処理を終了する。

以上のように、本実施の形態によれば、釣合い状態に対して本体がピッチ方向に傾斜しているピッチ傾斜角を精度良く推定することができるとともに、ピッチ用角速度センサによるピッチ角速度を積分することがないので、ノイズ、オフセット等の累積による目標ピッチ角の計算誤差が生じることがなく、車輪の回転に伴う反動トルクを利用し、釣合い状態からのピッチ方向の傾斜を精度良く補正して、ピッチ方向への転倒を防止することができる。また、車輪の前進又は後退指示を受け付け、受け付けた指示及びピッチ用回転センサで検出された回転角度から導出したピッチ方向の回転速度偏差に基づいて目標ピッチ角を算出することにより、指示された移動のための回転速度を確保しつつ、ピッチ方向の傾斜を補正することができる。さらに、加速度センサ等を別個に必要とせず、車輪の駆動に必要なモータ等の他は、角速度センサを加えただけの簡便な構成で、転倒防止制御装置を簡素化及び小型化することが可能である。

また、釣合い状態に対して本体がロール方向に傾斜しているロール傾斜角を精度良く推定することができるとともに、ロール用角速度センサによるロール角速度を積分することがないので、ノイズ、オフセット等の累積による目標ロール角の計算誤差が生じることがなく、慣性ロータの回転に伴う反動トルクを利用し、釣合い状態からのロール方向の傾斜を精度良く補正して、ロール方向への転倒を防止することができる。

なお、上述した実施の形態は、本発明の趣旨を逸脱しない範囲で変更することができることは言うまでもない。上述した実施の形態に係る転倒防止制御装置は、一輪車ロボットへの適用に限らず、例えば二足歩行ロボット、左右方向の１つの軸を中心に前後方向に回転して移動する二輪車等に適用しても良く、上述した効果と同様の効果を奏することができる。

Claims (12)

1. 前後方向に回転して移動する車輪と、該車輪の回転軸に連結されて該車輪の上方でピッチ方向及びロール方向に揺動する本体とで構成され、
   該本体に、
   ピッチ方向の傾斜角度の角速度であるピッチ角速度を検出するピッチ用角速度センサと、
   前記車輪の回転と連動し、該車輪を回転させるピッチ用モータと、
   該ピッチ用モータの回転位置又は回転速度を検出するピッチ用回転センサと
   を備え、
   前記車輪の回転に伴う反動トルクを利用して前記本体のピッチ方向の傾斜を補正する転倒防止制御装置であって、
   前記車輪の前進又は後退指示を受け付ける前進／後退指示受付部と、
   受け付けた前進又は後退指示及び前記ピッチ用回転センサで検出された回転角度から導出したピッチ方向の回転速度偏差に基づいて目標とするピッチ方向の傾斜角度である目標ピッチ角を算出する目標ピッチ角計算部と、
   前記ピッチ用角速度センサで検出されたピッチ角速度と、前記目標ピッチ角に基づいて生成される前記ピッチ用モータに与えるピッチ用トルク指令とから、釣合い状態に対して前記本体がピッチ方向に傾斜している角度であるピッチ傾斜角を推定するピッチ傾斜角推定部と、
   前記目標ピッチ角と前記ピッチ傾斜角とに基づいて、前記ピッチ用トルク指令を生成するピッチ用トルク指令生成部と
   を備えることを特徴とする転倒防止制御装置。
2. 前記前進／後退指示受付部は、前記車輪の前進又は後退指示を回転速度又は回転角度として受け付けることを特徴とする請求項１記載の転倒防止制御装置。
3. 前記目標ピッチ角から前記ピッチ傾斜角を減じたピッチ角度偏差から、目標とするピッチ方向の傾斜角度の角速度である目標ピッチ角速度を算出する目標ピッチ角速度計算部を備え、
   前記ピッチ用トルク指令生成部は、前記目標ピッチ角速度と検出された前記ピッチ角速度との偏差に基づいて、前記ピッチ用トルク指令を生成することを特徴とする請求項１又は２記載の転倒防止制御装置。
4. 前記ピッチ傾斜角に基づいて、前記本体をピッチ方向に傾斜させるピッチ方向外部トルクを推定するピッチ方向外部トルク推定部と、
   推定された前記ピッチ方向外部トルクが相殺される方向に前記ピッチ用トルク指令を補正するトルク補正部と
   を備えることを特徴とする請求項１乃至３のいずれか一項に記載の転倒防止制御装置。
5. 前記本体に、
   ロール方向の傾斜角度の角速度であるロール角速度を検出するロール用角速度センサと、
   ロール方向に回転する慣性ロータと、
   該慣性ロータの回転と連動し、該慣性ロータを回転させるロール用モータと、
   該ロール用モータの回転位置又は回転速度を検出するロール用回転センサと
   を備え、
   前記ロール用回転センサで検出されたロール方向の回転速度に基づいて目標とするロール方向の傾斜角度である目標ロール角を算出する目標ロール角計算部と、
   前記ロール用角速度センサで検出されたロール角速度と、前記目標ロール角に基づいて生成される前記ロール用モータに与えるロール用トルク指令とから、釣合い状態に対して前記本体がロール方向に傾斜している角度であるロール傾斜角を推定するロール傾斜角推定部と、
   前記目標ロール角から前記ロール傾斜角を減じたロール角度偏差から、目標とするロール方向の傾斜角度の角速度である目標ロール角速度を算出する目標ロール角速度計算部と、
   前記目標ロール角速度と検出された前記ロール角速度との偏差に基づいて、前記ロール用トルク指令を生成するロール用トルク指令生成部と
   を備えることを特徴とする請求項１乃至４のいずれか一項に記載の転倒防止制御装置。
6. 前記ロール用角速度センサの検出軸は、前記車輪の回転軸と交差する鉛直線上に前記本体の重心を有する状態で、前後方向かつ水平方向を向いていることを特徴とする請求項５記載の転倒防止制御装置。
7. 前記車輪の回転は、前記ピッチ用モータの回転に対して回転比が低くなっていることを特徴とする請求項１乃至６いずれか一項に記載の転倒防止制御装置。
8. 前後方向に回転して移動する車輪と、該車輪の回転軸に連結されて該車輪の上方でピッチ方向及びロール方向に揺動する本体とで構成され、
   該本体に、
   ピッチ方向の傾斜角度の角速度であるピッチ角速度を検出するピッチ用角速度センサと、
   前記車輪の回転と連動し、該車輪を回転させるピッチ用モータと、
   該ピッチ用モータの回転位置又は回転速度を検出するピッチ用回転センサと
   を備え、
   前記車輪の回転に伴う反動トルクを利用して前記本体のピッチ方向の傾斜を補正する転倒防止制御装置に搭載されているコンピュータで実行することが可能なコンピュータプログラムであって、
   前記コンピュータを、
   前記車輪の前進又は後退指示を受け付ける前進／後退指示受付手段、
   受け付けた前進又は後退指示及び前記ピッチ用回転センサで検出された回転角度から導出したピッチ方向の回転速度偏差に基づいて目標とするピッチ方向の傾斜角度である目標ピッチ角を算出する目標ピッチ角計算手段、
   前記ピッチ用角速度センサで検出されたピッチ角速度と、前記目標ピッチ角に基づいて生成される前記ピッチ用モータに与えるピッチ用トルク指令とから、釣合い状態に対して前記本体がピッチ方向に傾斜している角度であるピッチ傾斜角を推定するピッチ傾斜角推定手段、及び
   前記目標ピッチ角と前記ピッチ傾斜角とに基づいて、前記ピッチ用トルク指令を生成するピッチ用トルク指令生成手段
   として機能させることを特徴とするコンピュータプログラム。
9. 前記前進／後退指示受付手段を、前記車輪の前進又は後退指示を回転速度又は回転角度として受け付ける手段として機能させることを特徴とする請求項８記載のコンピュータプログラム。
10. 前記コンピュータを、
    前記目標ピッチ角から前記ピッチ傾斜角を減じたピッチ角度偏差から、目標とするピッチ方向の傾斜角度の角速度である目標ピッチ角速度を算出する目標ピッチ角速度計算手段
    として機能させ、
    前記ピッチ用トルク指令生成手段を、
    前記目標ピッチ角速度と検出された前記ピッチ角速度との偏差に基づいて、前記ピッチ用トルク指令を生成する手段
    として機能させることを特徴とする請求項８又は９記載のコンピュータプログラム。
11. 前記コンピュータを、
    前記ピッチ傾斜角に基づいて、前記本体をピッチ方向に傾斜させるピッチ方向外部トルクを推定するピッチ方向外部トルク推定手段、及び
    推定された前記ピッチ方向外部トルクが相殺される方向に前記ピッチ用トルク指令を補正するトルク補正手段
    として機能させることを特徴とする請求項８乃至１０のいずれか一項に記載のコンピュータプログラム。
12. 前記本体に、
    ロール方向の傾斜角度の角速度であるロール角速度を検出するロール用角速度センサと、
    ロール方向に回転する慣性ロータと、
    該慣性ロータの回転と連動し、該慣性ロータを回転させるロール用モータと、
    該ロール用モータの回転位置又は回転速度を検出するロール用回転センサと
    を備え、
    前記コンピュータを、
    前記ロール用回転センサで検出されたロール方向の回転速度に基づいて目標とするロール方向の傾斜角度である目標ロール角を算出する目標ロール角計算手段、
    前記ロール用角速度センサで検出されたロール角速度と、前記目標ロール角に基づいて生成される前記ロール用モータに与えるロール用トルク指令とから、釣合い状態に対して前記本体がロール方向に傾斜している角度であるロール傾斜角を推定するロール傾斜角推定手段、
    前記目標ロール角から前記ロール傾斜角を減じたロール角度偏差から、目標とするロール方向の傾斜角度の角速度である目標ロール角速度を算出する目標ロール角速度計算手段、及び
    前記目標ロール角速度と検出された前記ロール角速度との偏差に基づいて、前記ロール用トルク指令を生成するロール用トルク指令生成手段
    として機能させることを特徴とする請求項８乃至１１のいずれか一項に記載のコンピュータプログラム。

Images