(57)【要約】車体と、車体に同軸に配置された２つの車輪と、車輪を各々駆動する駆動装置と、車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、車体の回転が拘束されているかどうかを検出する車体拘束認識手段と、駆動装置への指令値を決定する制御手段を備え、車体拘束認識手段の検出結果に基づいて、制御手段が車体回転に寄与するトルクの割合を変化させた指令値にする。【特許請求の範囲】
【請求項１】
車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記第１状態検出手段又は上記第２状態検出手段で検出された情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを検出する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボット。
【請求項２】
上記車体に対する接触力を検出する少なくとも一つの接触センサを上記車体に有するとともに、上記車体拘束認識手段は、上記接触センサで検出された接触情報を入力として、車体拘束状態を認識する請求項１に記載の倒立二輪走行型ロボット。
【請求項３】
車体拘束時には、上記車輪を回転駆動させるトルクに、上記車体への拘束力による発生するトルク分を加算する制御方法に切り替える請求項１又は２に記載の倒立二輪走行型ロボット。
【請求項４】
車体拘束時には上記車体の傾斜角度及び傾斜角速度に対するフィードバックを停止させる制御に切り替える請求項１又は２に記載の倒立二輪走行型ロボット。
【請求項５】
車体拘束時には上記車体の傾斜角度及び傾斜角速度に対するフィードバックを停止させ、かつ、上記車体の走行のみを継続する制御に切り替える請求項４に記載の倒立二輪走行型ロボット。
【請求項６】
車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする請求項１又は２に記載の倒立二輪走行型ロボット。
【請求項７】
車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする請求項３に記載の倒立二輪走行型ロボット。
【請求項８】
車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする請求項４に記載の倒立二輪走行型ロボット。
【請求項９】
車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする請求項５に記載の倒立二輪走行型ロボット。
【請求項１０】
車体の傾斜角度及び傾斜角速度の少なくとも１つを第１状態検出情報として検出し、
上記車輪の回転角度及び回転角速度の少なくとも１つを第２状態検出情報として検出し、
上記第１状態検出情報又は上記第２状態検出情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを車体拘束認識情報として検出し、
上記検出された車体拘束認識情報に基づいて、制御手段が複数の走行制御方法を切替えて、上記車体に同軸に配置された２つの車輪を各々駆動する駆動装置への指令値を決定し、決定された上記指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボットの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】  本発明は、荷物運搬作業を行うロボットのうち、同軸二輪車を用いた倒立二輪走行型ロボット及びその制御方法に関するものであり、詳しくは、ロボット姿勢拘束時の倒立二輪走行型ロボット及びその制御方法に関するものである。
【背景技術】
【０００２】  荷物運搬作業を行うロボットしては、三輪型や四輪型など３点以上で必ず接触して安定を保つものが一般的である。しかしながら、三輪型、四輪型などは、面積を有すること、小回りが難しいことから倒立振子モデルに基づいて動作する同軸二輪車型のロボットが研究されてきている。【０００３】  同軸二輪車型の移動ロボットとしては、例えば特許文献１、特許文献２に記載のものが知られている。【０００４】  図１３は、上記特許文献１に記載された従来の同軸二輪車型の移動ロボットを示すものである。【０００５】  特許文献１に記載された同軸二輪車型移動ロボットは、両端に一対の車輪１０２，１０３を備えた車軸１０１上の回動可能に支持された車体１０４と、車体１０４に装着された車輪駆動用モータ１０５と、車輪駆動用モータ１０５に作動指令を送る制御コンピュータ１０６と、車体１０４の傾きを検出する角度検出手段１０７を備えている。そして、角度検出手段１０７により検出される車体１０４の傾斜角度を短時間間隔にてサンプリングし、車体１０４の傾斜角度を状態変数入力値、フィードバックゲインＫを係数として、制御コンピュータ１０６内に予め入力設定された制御入力算出式に基づき、車輪駆動用モータ１０５の制御トルクを算出する状態フィードバック制御である。この算出された制御トルク相当の作動を制御コンピュータ１０６から、車輪駆動用モータ１０５に指令することにより、倒立の姿勢制御が維持される。【０００６】  図１４は、上記特許文献２に記載された人乗り型の同軸二輪車型移動ロボットを示すものである。【０００７】  特許文献２に記載された人乗り型の同軸二輪車型移動ロボットは、プラットホーム２０１上に同軸に配された左右の駆動輪２０２を姿勢感知センサーの出力に応じて制御駆動することで前後方向のバランスの保持のための姿勢制御と走行制御とを行う。そして、駆動輪２０２の前方側又は及び後方側で接地する補助輪２０３と、補助輪２０３を出没させる補助輪駆動部２０４とを備えている。そして、障害物検知センサー２０５と走行速度及び姿勢の制御の状態に応じて補助輪駆動部２０４を駆動して補助輪２０３を出没させる制御回路を備えているため、転倒を招く原因となりやすい障害物があった時、補助輪２０３が出て安定性を高められるために、転倒の危険性を少なくすることができる。【０００８】
【特許文献１】特許２５３０６５２号
【特許文献２】特開２００４−７４８１４
【発明の開示】
【発明が解決しようとする課題】
【０００９】  上述の特許文献１に示された技術においては、状態フィードバック制御を常に行っている。【００１０】  そして、同軸二輪車の車体１０４と車輪１０２，１０３が共に拘束無く相対的に回転できる場合には、車輪駆動用モータ１０５で発生したトルクは、車輪１０２，１０３の回転に寄与すると同時に、反作用のトルクが車体１０４に働いて、全体として倒立姿勢制御を維持できる。【００１１】  しかしながら、車体１０４の一部が壁や地面に接触し、それ以上回転できなくなるような拘束力を外部から受けると車体１０４が回転しないだけでなく、拘束力によるトルクが車輪１０２，１０３に伝達される。よって、拘束力がない場合に比べて、車輪１０２，１０３に対して過大なトルクがかかり、車輪回転が増し、走行速度が急上昇するという課題を有していた。【００１２】  上述の特許文献２に示された技術においては、障害物検知センサー２０５で障害物を検知した場合、補助輪駆動部２０４を駆動して補助輪２０３を出没させる機構を有しているため、補助輪２０３が出没した場合には、左右の駆動輪２０２と補助輪２０３の３点接地になり、転倒の危険性を少なくできる。【００１３】  しかし、本技術では障害物を認識して補助輪２０３を出すだけで、速度や姿勢は人の操縦に依存している構成となっている。本構成においても、人がバランスを崩してプラットホーム２０１を含めた本体部が地面に接触した場合や、プラットホーム２０１を含めた本体部が壁等に接触して回転できなくなるような拘束力を受けた場合、特許文献１の場合と同様に駆動輪２０２に過大なトルクがかかり、走行速度が急上昇するという課題を有していた。この場合は、人の操縦により停止指令を送るなどの、危険を回避することになるが、操作が人に依存するという点でユーザに負担をかけるという課題を有していた。【００１４】  本発明の目的は、上記従来の課題を解決するもので、倒立制御を行いながら移動する同軸二輪車の車体の一部が地面や壁などに接触して拘束力を受けた場合に、姿勢制御や走行状態を維持可能でかつユーザに負担をかけない安全な倒立二輪走行型ロボット及びその制御方法を提供する。
【課題を解決するための手段】
【００１５】  上記目的を達成するために、本発明は以下のように構成する。【００１６】  本発明の第１態様によれば、車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記第１状態検出手段又は上記第２状態検出手段で検出された情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを検出する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボットを提供する。【００１７】  本発明の第１０態様によれば、車体の傾斜角度及び傾斜角速度の少なくとも１つを第１状態検出情報として検出し、
上記車輪の回転角度及び回転角速度の少なくとも１つを第２状態検出情報として検出し、
上記第１状態検出情報又は上記第２状態検出情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを車体拘束認識情報として検出し、
上記検出された車体拘束認識情報に基づいて、制御手段が複数の走行制御方法を切替えて、上記車体に同軸に配置された２つの車輪を各々駆動する駆動装置への指令値を決定し、決定された上記指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボットの制御方法を提供する。
【発明の効果】
【００１８】  本発明の構成によれば、倒立して姿勢制御を行いながら走行する倒立二輪走行型ロボットの車体の一部が、地面や壁などに接触して回転を拘束された場合に、車体拘束認識手段又は車体拘束認識動作によって車体の回転方向の拘束が認識される。そして、認識された拘束状態に応じて、例えば車体回転に寄与するトルクの割合を変化させることで過大なトルクが車輪にかかって姿勢制御を維持できず走行速度が急上昇するという課題を回避することができる。【００１９】  よって、本発明の構成によれば、倒立制御を行いながら移動する同軸二輪車型の倒立二輪走行型ロボットにおいて、車体の一部が地面や壁などに接触して回転を拘束された場合でも、姿勢制御や走行状態を維持でき、ユーザに負担をかけずに、安全に移動することができる。
【図面の簡単な説明】
【００２０】  本発明のこれらと他の目的と特徴は、添付された図面についての好ましい実施形態に関連した次の記述から明らかになる。この図面においては、【図１Ａ】図１Ａは、本発明の第１実施形態における倒立二輪走行型ロボットの傾いた状態での側面図であり、
【図１Ｂ】図１Ｂは、本発明の第１実施形態における倒立二輪走行型ロボットの制御部などのブロック図であり、
【図２】図２は、本発明の第１実施形態における倒立二輪走行型ロボットの傾いた状態での正面図であり、
【図３】図３は、本発明の第１実施形態における倒立二輪走行型ロボットの移動構成の説明図であり、
【図４】図４は、本発明の第１実施形態における倒立二輪走行型ロボットの機構パラメータ図であり、
【図５】図５は、本発明の第１実施形態における倒立二輪走行型ロボットの制御ブロック図であり、
【図６】図６は、本発明の第１実施形態における倒立二輪走行型ロボットの旋回制御模式図であり、
【図７】図７は、本発明の第１実施形態における走行制御全体図であり、
【図８】図８は、本発明の第２実施形態における倒立二輪走行型ロボットのフローチャートであり、
【図９】図９は、本発明の第１実施形態における倒立二輪走行型ロボットの接触状態図であり、
【図１０】図１０は、本発明の第２実施形態における倒立二輪走行型ロボットのフローチャートであり、
【図１１】図１１は、本発明の第３実施形態における倒立二輪走行型ロボットの車体が傾いて地面に必ず接触する場合の説明図であり、
【図１２】図１２は、本発明の第１実施形態における倒立二輪走行型ロボットのフローチャートであり、
【図１３】図１３は、従来の特許文献１に記載の同軸二輪車型の移動ロボットの図であり、
【図１４】図１４は、従来の特許文献２に記載の人乗り型の同軸二輪車型移動ロボットの図である。
【発明を実施するための最良の形態】
【００２１】  本発明の記述を続ける前に、添付図面において同じ部品については同じ参照符号を付している。【００２２】  以下に、本発明にかかる実施の形態を説明する前に、本発明の種々の態様について説明する。【００２３】  本発明の第１態様によれば、車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記第１状態検出手段又は上記第２状態検出手段で検出された情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを検出する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボットを提供する。【００２４】  本発明の第２態様によれば、上記車体に対する接触力を検出する少なくとも一つの接触センサを上記車体に有するとともに、上記車体拘束認識手段は、上記接触センサで検出された接触情報を入力として、車体拘束状態を認識する第１の態様に記載の倒立二輪走行型ロボットを提供する。【００２５】  本発明の第３態様によれば、車体拘束時には、上記車輪を回転駆動させるトルクに、上記車体への拘束力による発生するトルク分を加算する制御方法に切り替える第１又は２の態様に記載の倒立二輪走行型ロボットを提供する。【００２６】  本発明の第４態様によれば、車体拘束時には上記車体の傾斜角度及び傾斜角速度に対するフィードバックを停止させる制御に切り替える第１又は２の態様に記載の倒立二輪走行型ロボットを提供する。【００２７】  本発明の第５態様によれば、車体拘束時には上記車体の傾斜角度及び傾斜角速度に対するフィードバックを停止させ、かつ、上記車体の走行のみを継続する制御に切り替える第４の態様に記載の倒立二輪走行型ロボットを提供する。【００２８】  本発明の第６態様によれば、車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする第１又は２の態様に記載の倒立二輪走行型ロボットを提供する。【００２９】  本発明の第７態様によれば、車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする第３の態様に記載の倒立二輪走行型ロボットを提供する。【００３０】  本発明の第８態様によれば、車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする第４の態様に記載の倒立二輪走行型ロボットを提供する。【００３１】  本発明の第９態様によれば、車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする第５の態様に記載の倒立二輪走行型ロボットを提供する。【００３２】  本発明の第１０態様によれば、車体の傾斜角度及び傾斜角速度の少なくとも１つを第１状態検出情報として検出し、
上記車輪の回転角度及び回転角速度の少なくとも１つを第２状態検出情報として検出し、
上記第１状態検出情報又は上記第２状態検出情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを車体拘束認識情報として検出し、
上記検出された車体拘束認識情報に基づいて、制御手段が複数の走行制御方法を切替えて、上記車体に同軸に配置された２つの車輪を各々駆動する駆動装置への指令値を決定し、決定された上記指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボットの制御方法を提供する。【００３３】  以下に、本発明にかかる実施の形態を図面に基づいて詳細に説明する。【００３４】  （第１実施形態）
以下、本発明の第１実施形態における倒立二輪走行型ロボット及びその制御方法について、図面を参照しながら説明する。【００３５】  本発明の第１実施形態における倒立二輪走行型ロボットの傾いた状態での側面図を図１Ａ、傾いた状態での正面図を図２に示す。ロボットの車体１に対し、概して同一軸上に２つの車輪２ａ，２ｂが配置される。ここで、車体１の図中矢印Ａで示した進行方向に対して左車輪２ａ、右車輪２ｂとする。なお、車体１の進行方向から見た正面図が図２である。車輪２ａ，２ｂと車体１との間に独立してそれぞれ駆動力を発生させる駆動装置の一例として機能するアクチュエータ３ａ，３ｂは車体１の車輪２ａ，２ｂの近傍に取付けられ、各車輪２ａ，２ｂに連結されている。なお、説明のためアクチュエータ３ａ，３ｂは一例としてモータであるとする。なお、モータ３ａ，３ｂには、減速機４ａ，４ｂが取り付けられて、上記駆動装置の一例を構成するようにしてもよい。また、モータ３ａ，３ｂには、上記車輪２ａ，２ｂの回転角度及び回転角速度の少なくとも１つを第２状態検出情報として検出する第２状態検出手段の一例として機能するエンコーダ５ａ，５ｂが取り付けられており、車体１と車輪２ａ，２ｂの回転角度を計測できる。回転角度を一定サンプリング時間（例えば、１ｍｓｅｃおき）に計測し、その差分をサンプリング時間で除算することで、回転角速度が求まり、これを第２状態検出情報として扱うことができる。【００３６】  また、車体１の車軸方向を法線とする鉛直平面内における姿勢を検出する姿勢センサ６の一例である２つのセンサ、すなわち、車体１の傾斜角度及び傾斜角速度の少なくとも１つを第１状態検出情報として検出する第１状態検出手段の一例として機能する１軸ジャイロ６ａと２軸加速度センサ６ｂが車体１の上部に取り付けられている。１軸ジャイロ６ａは、車軸方向を法線とする鉛直平面に、車軸方向の傾斜角速度を第１状態検出情報として検出できるように車体１に取り付けられ、車体１の車軸方向を法線とする鉛直平面内における傾斜角速度（円弧６ａ１で図示）を検出することができる。また、２軸加速度センサ６ｂは、車軸方向を法線とする鉛直平面の所定の２方向（６ｂ１，６ｂ２）に対する加速度を検出できるように取り付けられ、重力方向（６ｂ３）を検出することを可能としている。第１状態検出情報としての車体１の姿勢角度（傾斜角度）は、１軸ジャイロ６ａの積分値と２軸加速度センサ６ｂからの値をそれぞれ、制御コンピュータ部９でハイパスフィルター、ローパスフィルターにかけ、それらを制御コンピュータ部９で合成して求める。【００３７】  また、車体１の前後の下部には接触センサ１０ａ，１０ｂが設けられ、車体１の当該車体１の前後の下部に対する他の物体などからの接触力を、接触情報としてそれぞれ測定することができる。【００３８】  車体１の内部には、モータ３ａ，３ｂを駆動するためのモータドライバ７ａ，７ｂ及びバッテリ８、制御手段及び車体拘束認識手段の一例して機能する制御コンピュータ部９が搭載されている。図１Ｂに示すように、制御コンピュータ部９は、Ａ／Ｄ変換器９ａとＤ／Ａ変換器９ｂとエンコーダカウンタ部９ｃと演算部９ｄとから構成され、エンコーダ５ａ，���ｂの値はエンコーダカウンタ部９ｃを通じて演算部９ｄに入力される。また、１軸ジャイロ６ａと２軸加速度センサ６ｂ、接触センサ１０ａ，１０ｂの値はＡ／Ｄ変換器９ａを通じて演算部９ｄに入力される。演算部９ｄでは、入力された接触センサ１０ａ，１０ｂからの値に基づき、上記車体１の傾斜方向への回転が拘束されているかどうかを検出して、車体拘束認識情報を作成することができる。この作成された車体拘束認識情報は、例えば、接触センサ１０ａ，１０ｂの入力により判断される車体１に対する拘束力の情報（第１実施形態参照）、接触センサ１０ａ，１０ｂの入力により判断される接触の有無の情報（第２実施形態参照）、車体１の傾斜角度情報（第３実施形態参照）などであって、後述するように、制御方法を切替えるときに使用することができる。制御コンピュータ部９の演算部９ｄは、演算部９ｄに入力された値より、姿勢制御及び走行制御の必要トルクを計算し、Ｄ／Ａ変換器９ｂより、トルク指令値をモータドライバ７ａ，７ｂに指令する。モータドライバ７ａ，７ｂは、指令値に基づいてモータ３ａ，３ｂを駆動する。なお、入力値から出力値への計算方法は、後述する。【００３９】  次に、本構成での倒立二輪走行型ロボットの制御方法について図３で説明する。図３はロボットを鉛直上方から見た図である。本機構においては、車軸方向を法線とする鉛直平面（Ｂ−Ｂ’面）内での姿勢及びその鉛直平面内の水平方向速度に関する並進制御１１（姿勢制御＋前後移動制御）と旋回方向の動作に関する旋回制御１２の２つの制御に分けて考える。前者は、並進方向の
【数１】
2007013282.tif 000002
に追従するように、姿勢制御と走行制御を行って走行状態を維持するものである。後者は、旋回方向の
【数２】
2007013282.tif 000003
に追従するように制御するものである。後述するように、この２つの制御を合わせることで、移動制御（走行制御）を行って走行状態を維持することができる。【００４０】  次に、この移動制御について、順に、説明を行う。【００４１】  まず、並進制御１１について、機構パラメータを示す図４を用いて説明する。なお、車軸方向を法線とする鉛直平面内での動作であるので、２次元モデルとして扱う。図４のように、パラメータを以下のように設定する。車輪２ａ，２ｂ、モータ３ａ，３ｂ、減速機４ａ，４ｂ、エンコーダ５ａ，５ｂ、はそれぞれ１つとみなして、車輪２、モータ３、減速機４、エンコーダ５とする。また、車体１の重心を車体重心１３、車輪２の車軸を車軸１４、車輪２の重心を車輪重心１５とする。【００４２】    Ｍｂ：ロボットの車体１の質量［ｋｇ］
Ｍｗ：車輪２の質量（２つの車輪の合計）［ｋｇ］、
Ｉｂ：ロボットの車体１の車体重心１３周りの慣性モーメント［ｋｇｍ２］
Ｉｗ：車輪２の車輪重心１５周りの慣性モーメント（２つの車輪の合計）［ｋｇｍ２］
Ｉｍ：モータ３の慣性モーメント［ｋｇｍ２］
ｒ：車輪２の半径［ｍ］
Ｌｇ：車軸１４からロボットの車体重心１３までの距離［ｍ］【００４３】
【数３】
2007013282.tif 000004
：モータ３と減速機４と車軸２との間の粘性摩擦係数［Ｎｍ／（ｒａｄ／ｓ）］
【数４】
2007013282.tif 000005
：車輪２と地面８００との間の粘性摩擦係数［Ｎｍ／（ｒａｄ／ｓ）］
【数５】
2007013282.tif 000006
：モータ３のトルク定数［Ｎｍ／Ａ］
【数６】
2007013282.tif 000007
：減速機４のギア部減速比
ｇ：重力加速度
【数７】
2007013282.tif 000008
：ロボットの車体１の傾き［ｒａｄ］
【数８】
2007013282.tif 000009
：車輪２の地面８００に対する回転角［ｒａｄ］
ｕ：モータ３に入力する電流［Ａ］【００４４】  また、記号を次のように定義する。
Ｔ：運動エネルギー、Ｕ：位置エネルギー，Ｄ：摩擦損失
ｚ１：車体重心高さ，ｚ２：車輪重心高さ，ｓ１：車体重心並進位置，
ｓ２：車軸並進位置，
ここで、倒立２輪走行型ロボットの運動方程式を、ラグランジェ法を用いて導出する。ラグランジェの運動方程式は
【数９】
2007013282.tif 000010
【数１０】
2007013282.tif 000011
各値とその微分値は次のように表される。
【数１１】
2007013282.tif 000012
【数１２】
2007013282.tif 000013
【数１３】
2007013282.tif 000014
【数１４】
2007013282.tif 000015
【数１５】
2007013282.tif 000016
【数１６】
2007013282.tif 000017
【数１７】
2007013282.tif 000018
【数１８】
2007013282.tif 000019
各エネルギーは
【数１９】
2007013282.tif 000020
【数２０】
2007013282.tif 000021
【数２１】
2007013282.tif 000022
簡単なモデルとして、車体１にかかる外力が０とした場合、
【数２２】
2007013282.tif 000023
【数２３】
2007013282.tif 000024
よって、運動エネルギーＴは
【数２４】
2007013282.tif 000025
となる。【００４５】  ロボットの車体１の傾きφについて、ラグランジェ法により運動方程式を導出する。
【数２５】
2007013282.tif 000026
【００４６】  車輪２の地面８００に対する回転角θについて、ラグランジェ法により運動方程式を導出する。
【数２６】
2007013282.tif 000027
（式１７）と（式１８）を足し合わせると、
【数２７】
2007013282.tif 000028
となる。【００４７】  よって、２つの式（式１７）と（式１９）を
【数２８】
2007013282.tif 000029
の周りで近似して
【数２９】
2007013282.tif 000030
【数３０】
2007013282.tif 000031
となる。【００４８】  ここで変数を
【数３１】
2007013282.tif 000032
【数３２】
2007013282.tif 000033
とおいて、
【数３３】
2007013282.tif 000034
【数３４】
2007013282.tif 000035
の状態方程式の形に帰着できる。なお、
【数３５】
2007013282.tif 000036
は、状態ベクトルである。【００４９】  以上のモデル化は、状態ベクトルの各値が０での近似モデルであるので、前後方向の位置（速度指令）に関する追従性を考慮するために、このモデルに対してステップ状の目標入力に追従するモデルの制御系を構成する。なお、目標入力を一定速度目標値であるとする。
【数３６】
2007013282.tif 000037
は、可観測であるとし、
【数３７】
2007013282.tif 000038
と表せる。【００５０】
【数３８】
2007013282.tif 000039
をステップ状の
【数３９】
2007013282.tif 000040
に定常偏差なく追従させるために、積分器を用いた制御系とする。なお、そのときの制御ブロックを図５に示す。ここで、
【数４０】
2007013282.tif 000041
の閉ループシステムを安定にするためのフィードバックベクトルである。【００５１】  この構成において、
【数４１】
2007013282.tif 000042
より
【数４２】
2007013282.tif 000043
制御入力は、
【数４３】
2007013282.tif 000044
と表せる。ここで、Ｉｎは３行３列の単位行列、Ｉｍ＝１として
【数４４】
2007013282.tif 000045
（式２６）と（式３０）より
【数４５】
2007013282.tif 000046
【数４６】
2007013282.tif 000047
ここで、定常値は、
【数４７】
2007013282.tif 000048
【数４８】
2007013282.tif 000049
なお、
【数４９】
2007013282.tif 000050
となり、定常偏差がないことがわかる。【００５２】  次に、定常値からの誤差システムを考える。
【数５０】
2007013282.tif 000051
として式変形を行うと
【数５１】
2007013282.tif 000052
となる。【００５３】
【数５２】
2007013282.tif 000053
【数５３】
2007013282.tif 000054
として、
【数５４】
2007013282.tif 000055
となり、出力をｅとして
【数５５】
2007013282.tif 000056
となる。（式３９）、（式４０）にｗの状態フィードバックをしたレギュレータとみなせる。よって、この誤差システムのフィードバックゲインＨを、最適レギュレータ法や極配置法、その他の種々の設計法によって導出する。このゲインを用いれば、
【数５６】
2007013282.tif 000057
の過渡応答誤差の少ない最適なサーボシステムが設計できる。なお、（式３７）より
【数５７】
2007013282.tif 000058
となる。したがって、
【数５８】
2007013282.tif 000059
を用いて、モータ３へ出力される
【数５９】
2007013282.tif 000060
を、
【数６０】
2007013282.tif 000061
とすれば、倒立の姿勢制御と速度制御が同時に実現できる。【００５４】  ただし、
【数６１】
2007013282.tif 000062
【数６２】
2007013282.tif 000063
である。なお、
【数６３】
2007013282.tif 000064
である。本構成では、車輪２は２つであるので、
【数６４】
2007013282.tif 000065
の０．５倍を各車輪２（２ａ，２ｂ）に出力すれば、並進方向の姿勢制御と速度制御を行える。そのときの、各車輪２ａ，２ｂの
【数６５】
2007013282.tif 000066
として、
【数６６】
2007013282.tif 000067
となる。【００５５】  次に、旋回制御１２について旋回制御模式図である図６を用いて説明する。【００５６】  ロボットの車体１を上面から見て、ロボットの車体１の車輪２間の長さを
【数６７】
2007013282.tif 000068
とし、水平面上でのロボットの車体１の移動速度を
【数６８】
2007013282.tif 000069
とし、左車輪２ａの回転速度を
【数６９】
2007013282.tif 000070
とし、右車輪２ｂの回転速度を
【数７０】
2007013282.tif 000071
とし、旋回半径をＲ、旋回速度を
【数７１】
2007013282.tif 000072
とする。また、左車輪２ａの回転角速度を
【数７２】
2007013282.tif 000073
、右車輪２ｂの回転角速度を
【数７３】
2007013282.tif 000074
とすると、
【数７４】
2007013282.tif 000075
となる。【００５７】
【数７５】
2007013282.tif 000076
として、
【数７６】
2007013282.tif 000077
との差に対してフィードバック（ＰＤ制御等）を行い、
【数７７】
2007013282.tif 000078
を決定する。【００５８】
【数７８】
2007013282.tif 000079
は、左右の車輪２ａ，２ｂに発生するトルク差であるとみなせる。左右の車輪２ａ，２ｂに、それぞれ、その旋回方向のトルクの０．５倍を、旋回方向の目標トルクを算出できる両車輪２ａ，２ｂに、それぞれ加算、減算してやることで、旋回方向の制御が実現できる。【００５９】  並進制御１１、旋回制御１２の統合された制御ブロック図を図７に示す。【００６０】  そして、左輪２ａ、右輪２ｂに指令するトルク指令値を
【数７９】
2007013282.tif 000080
とすると
【数８０】
2007013282.tif 000081
【００６１】  以上のように、トルクを決定することにより、上記ロボットは、倒立姿勢を維持しながら旋回及び直線走行をすることができる。【００６２】  図１Ａのように、車体１が地面８００から離れている場合には、これまで述べたような制御則に基づけば、倒立姿勢を維持しながらの走行を行うことができる。【００６３】  次に、第１実施形態では、図８のように車体１の一部が回転方向に拘束を受けた場合の一例として、車体１が傾いて車体下部が地面８００に接触した場合を示す。仮定として、車体１と地面８００との摩擦力は０で
【数８１】
2007013282.tif 000082
だけが、車軸１４から水平移動した
【数８２】
2007013282.tif 000083
の接触点において拘束力として発生するとする。接触センサ１０ｂ（接触センサ１０ａも同様であるため、代表的に接触センサ１０ｂで説明する。）は、車体１に対する他の物体や地面８００や壁などからの接触力を計測できるセンサとし、接触力センサ１０ｂによって
【数８３】
2007013282.tif 000084
が検出され、幾何学的な条件より
【数８４】
2007013282.tif 000085
も既知である。このとき、車体１には拘束力による
【数８５】
2007013282.tif 000086
が発生する。また、位置ずれ量が
【数８６】
2007013282.tif 000087
の状態のときに、図８のような
【数８７】
2007013282.tif 000088
接触が発生した場合を考える。このとき、（式４２）に基づくモータ３のトルク指令値は、車輪２を正転させる方向（ロボットの車体１が図８において右方向に移動し、車体１が車軸１４周りに左回転する方向）を正として、
【数８８】
2007013282.tif 000089
となる。【００６４】
【数８９】
2007013282.tif 000090
であるので、傾斜方向の回転が拘束され、
【数９０】
2007013282.tif 000091
となる。
このとき
【数９１】
2007013282.tif 000092
となる。【００６５】
【数９２】
2007013282.tif 000093
は、モータ３からのトルクの
【数９３】
2007013282.tif 000094
と、
【数９４】
2007013282.tif 000095
の和になるため、
【数９５】
2007013282.tif 000096
となる。この
【数９６】
2007013282.tif 000097
の値によっては、
【数９７】
2007013282.tif 000098
のいずれにもなりうる。【００６６】  （式５１）において
【数９８】
2007013282.tif 000099
である場合には、車体１には、右回転させるようにするトルクが働くため、車軸１４周りに右回転し、接触は解消され倒立状態に戻る事ができる。しかしながら、
【数９９】
2007013282.tif 000100
である場合には、車体１には、左回転のトルクがかかり、車輪２には、反力として右へ移動する回転トルクが発生する。この時、車体１が
【数１００】
2007013282.tif 000101
で拘束されているので、車輪２には、
【数１０１】
2007013282.tif 000102
によって発生した
【数１０２】
2007013282.tif 000103
も、車輪２への回転トルクとして伝達される。そのため、車輪２に余分のトルクがかかり、車輪２の過度の回転現象を生じ、暴走現象に陥ってしまう。【００６７】  そこで、本発明の上記第１実施形態にかかるロボットの制御方法では、接触力センサ１０ｂより接触情報を検知し、検知した接触力方向と
【数１０３】
2007013282.tif 000104
とを比較し、その回転方向が反対の場合には、モータ３への
【数１０４】
2007013282.tif 000105
を、接触力により発生する
【数１０５】
2007013282.tif 000106
を加算した
【数１０６】
2007013282.tif 000107
とする。【００６８】  したがって、本発明による上記第１実施形態にかかるロボットの制御方法によれば、（式５２）に基づきモータ３のトルクを算出することにより、地面８００からの接触力による影響を除去でき、ユーザに負担をかけずに、安全な倒立二輪走行型ロボットを実現できる。【００６９】  また、本構成において車体１と地面８００との間に
【数１０７】
2007013282.tif 000108
が発生する場合、図９のように、その
【数１０８】
2007013282.tif 000109
によって発生する回転の反対方向のトルクを補償して走行することとすれば同様の効果を得られる
【数１０９】
2007013282.tif 000110
また、外乱オブザーバを用いて、接触センサ無しに外乱力を推定してその推定された接触力を用いて（式５２）、（式５３）と同様の制御を行ってもよい。【００７０】  ここで、上記した第１実施形態での切替制御を用いた走行動作フローチャートを図１２で説明する。【００７１】  初期状態（ステップＳ１）では、ロボットの車体１は傾斜して停止している。初期設定として、
【数１１０】
2007013282.tif 000111
（＝ステップＳ２）とする。次に、（式５０）で示す倒立停止
【数１１１】
2007013282.tif 000112
制御（ステップＳ３）を行って起き上がる。倒立停止制御（ステップＳ３）の安定時間（例えば５秒程度）後（ステップＳ４）、車体１の地面８００などに対する接触の有無を判別する（ステップＳ５）。【００７２】  ステップＳ５で接触が無い場合には、
【数１１２】
2007013282.tif 000113
設定を行った（ステップＳ８）後、（式４９）で示す倒立走行制御（ステップＳ９）を行って走行状態を維持した後、ステップＳ５に戻る。【００７３】  ステップＳ５で接触がある場合には、
【数１１３】
2007013282.tif 000114
設定を行った（ステップＳ６）後、（式５２又は式５３）で示す倒立走行制御（ステップＳ７）を行って走行状態を維持した後、ステップＳ５に戻る。【００７４】  なお、ステップＳ６とステップＳ８において、接地しているときも、接地していないときも、その差分トルクを補正するので、どちらの場合も、同様の走行が行える。【００７５】  以上のように、上記第１実施形態によれば、制御コンピュータ部９の制御の下で、以下のような動作制御を行うことができる。【００７６】  すなわち、初期の車体１が移動停止して地面８００に接地している状態から、まず、倒立停止制御（式（４９）、ステップＳ２〜Ｓ３）を行うことにより、車体１を倒立位置まで起き上がらせて、倒立状態（ただし、走行は停止している状態。）を維持する。次いで、所定時間（安定時間）経過して倒立状態が安定した（ステップＳ４）のち、接触センサ１０ａ，１０ｂによる接触の有無の検出を開始する（ステップＳ５）。ステップＳ５で接触（言い換えれば、車体拘束）が無ければ、倒立停止制御から倒立走行制御（式（４９）、ステップＳ８〜Ｓ９）に移って走行状態を維持したのち、接触センサ１０ａ，１０ｂによる接触の有無の検出を再度行う（ステップＳ５）。ステップＳ５で接触（言い換えれば、車体拘束）が検出された場合には、倒立停止制御から倒立走行制御（式（５２）又は式（５３）、ステップＳ６〜Ｓ７）に移って走行状態を維持したのち、接触センサ１０ａ，１０ｂによる接触の有無の検出を再度行う（ステップＳ５）。【００７７】  このように、倒立停止制御（ステップＳ３）、倒立走行制御（ステップＳ９）、倒立走行制御（ステップＳ７）とを適宜切替ながら、倒立二輪走行することによって、車体１に接触が発生しても、接触状態（すなわち車体拘束状態）に応じて、車体回転に寄与するトルクの割合を変化させることで、過大なトルクが車輪２にかかって姿勢制御を維持できず走行速度が急上昇するといった暴走を防止することができ、ユーザに負担をかけずに、安全に走行を継続することができる。【００７８】  （第２実施形態）
本発明の第２実施形態を第１実施形態と同じ、図８で説明する。第１実施形態においては、接触センサを設けて接触力を検知していたが、第２実施形態では、接触センサ１０ｂが、接触の有無を検知のみで力測定ができないセンサの場合や、力計測できるセンサであっても接触の有無のみを利用する場合とし、その制御方法を述べる。【００７９】  車体１の一部が地面８００などに接触している場合、車体１は回転できず、車体１は２つの車輪２ａ，２ｂと、少なくとも１点との３点以上で地面８００に接していることとなる。従来からあるような独立二輪駆動で、補助輪等で接地している台車の移動制御においては、車輪２の回転速度に対して、ＰＩＤ制御等によって走行制御がなされて走行状態を維持している。そのため、この第２実施形態においても、接地時には同様の構成とみなすことができるので、接地した場合には、
【数１１４】
2007013282.tif 000115
に示すような、回転速度に対する略ＰＩ制御の速度制御の構成とする。したがって、接触がある場合には、（式５４）、接触がない場合には（式５０）の制御を切り替えて制御を行う。【００８０】  第２実施形態での切替制御を用いた走行動作フローチャートを図１０で説明する。【００８１】  初期状態（ステップＳ１１）では、ロボットの車体１は傾斜して停止している。初期設定として、
【数１１５】
2007013282.tif 000116
（＝ステップＳ１２）とする。次に、（式５０）で示す倒立停止
【数１１６】
2007013282.tif 000117
制御（ステップＳ１３）を行って起き上がる。倒立停止制御（ステップＳ１３）の安定時間（例えば５秒程度）後（ステップＳ１４）、接触の有無を判別する（ステップＳ１５）。もし接触（ステップＳ１５）のない場合は、
【数１１７】
2007013282.tif 000118
設定を行った（ステップＳ２０）後、（式４９）で示す倒立走行制御（ステップＳ２１）を行って走行状態を維持している。（式４９）に基づく倒立走行制御（ステップＳ２１）から、接触時の（式５４）に基づく速度制御（ステップＳ１８）へと制御方法が移る条件として、積分項リセット履歴ビットｂｔ１を設ける。積分項とは、速度差の
【数１１８】
2007013282.tif 000119
（車体１の現在位置に対して車体１のバランスが崩れたりなどして位置ずれが生じるときの位置ずれ量）である。そして、倒立走行制御（ステップＳ２１）中に、接触の有無を判断する（ステップＳ２２）。【００８２】  ステップＳ２２で車体１が地面８００などに接触していない場合には、積分項リセット履歴ビットｂｔ１をリセット（ｂｔ１＝０）した（ステップＳ２６）後（すなわち、積分項のリセットは行わずに）、ステップＳ２０に戻る。【００８３】  ステップＳ２２で車体１が地面８００などに接触している場合には、積分項リセット履歴がＯＮか否か、すなわち、積分項リセット履歴ビットｂｔ１＝１か否かを判断する（ステップＳ２３）。積分項リセット履歴ビットｂｔ１＝０のとき（接触後に積分項のリセットを行っていなかったとき）のみ、
【数１１９】
2007013282.tif 000120
を
【数１２０】
2007013282.tif 000121
した（ステップＳ２７）後（積分項をリセットし、かつ、積分項をリセットしたことを示す履歴情報を残した後）、ステップＳ２０に戻る。積分項をリセットするのは、接触する前に発生している位置ずれ（速度差の
【数１２１】
2007013282.tif 000122
）を、一度、０にして、接触時からのＰＩ制御にするためである。このように、積分項をリセットすることによって、接触前の位置ずれ量を削除できて、接触後の車体１の位置ずれ量のみにより、動作制御が行われるため、車体１の暴走現象を防止することができる。
ステップＳ２３で積分項リセット履歴ビットｂｔ１＝１であるとき、接触の有無を判別する（ステップＳ２４）。【００８４】  ステップＳ２４で車体１が地面８００などに接触していない場合は、積分項リセット履歴ビットｂｔ１をリセット（ｂｔ１＝０）した（ステップＳ２８）後（すなわち、積分項のリセットは行わずに）、ステップＳ２０に戻る。【００８５】  ステップＳ２４で、そのまま、車体１が地面８００などに接触が続いている場合（ｂｔ１＝１）は、安定時間（例えば１秒）（ステップＳ２５）以内であれば、ステップＳ２０に戻り、倒立走行（ステップＳ２１）を継続する。【００８６】  一方、安定時間（例えば１秒）が経過していれば、倒立走行（ステップＳ２１）から速度制御（ステップＳ３０〜Ｓ３２）に移る。【００８７】  ステップＳ１５で車体１が地面８００などに接触している場合又はステップＳ２５で安定時間経過時には、
【数１２２】
2007013282.tif 000123
設定を行った（ステップＳ３０）後、
【数１２３】
2007013282.tif 000124
か否かを判断する（ステップＳ３１）。ＹＥＳの場合には、起き上がれないと判断して終了する（ステップＳ１９）。ステップＳ３１で、ＮＯ、すなわち、
【数１２４】
2007013282.tif 000125
の場合には、（式５４）で示す速度制御（ステップＳ３２）で走行したのち、ステップＳ１４に戻り、速度制御（ステップＳ３２）の安定時間（例えば５秒程度）が経過したか否かを判断する（ステップＳ１４）。【００８８】  以上のように、上記第２実施形態によれば、制御コンピュータ部９の制御の下で、以下のような動作制御を行うことができる。【００８９】  すなわち、初期の車体１が移動停止して地面８００に接地している状態から、まず、倒立停止制御（式（４９）、ステップＳ１２〜Ｓ１３）を行うことにより、車体１を倒立位置まで起き上がらせて、倒立状態（ただし、走行は停止している状態。）を維持する。次いで、所定時間（安定時間）経過して倒立状態が安定した（ステップＳ１４）のち、接触センサ１０ａ，１０ｂによる接触の有無の検出を開始する（ステップＳ１５）。ステップＳ１５で接触（言い換えれば、車体拘束）が無ければ、倒立停止制御から倒立走行制御（式（４９）、ステップＳ２０〜Ｓ２１）に移って走行状態を維持する。ステップＳ１５で接触（言い換えれば、車体拘束）が検出された場合には、倒立停止制御から速度制御（ステップＳ３０〜Ｓ３２）に移る。次いで、所定時間（安定時間）経過して安定して速度制御が行われた（ステップＳ１４）のち、接触センサ１０ａ，１０ｂによる接触の有無の検出を再度行い（ステップＳ１５）、ステップＳ１５で接触がなければ（起き上がりが成功したならば）、倒立走行制御（式（４９）、ステップＳ２０〜Ｓ２１）に移って走行状態を維持する。ステップＳ１５で接触があれば（起き上がりが不成功ならば）、そのまま、速度制御（ステップＳ３０〜Ｓ３２）を継続する。速度制御（ステップＳ３０〜Ｓ３２）を行うとき、速度及び角速度が０の場合には、車体１が物体又は地面８００などに接触している場合など、車体１が起き上がれない場合であると判断して、車体１の起き上がり駆動を停止し、接地状態とする。【００９０】  また、倒立走行制御（式（４９）、ステップＳ２０〜Ｓ２１）中に接触があった場合（ステップＳ２２）には、接触時までの位置ずれ量をリセットして０にしたのち、接触時からの位置ずれ量のみにより、動作制御が行われるため、車体１の暴走現象を防止することができる。さらに、倒立走行制御時に、接触時までの位置ずれ量をリセットして０にしたのち、接触が安定時間経過しても続いている場合には、車体１の倒立状態が崩れていると判断して、倒立走行制御から速度制御に切り替えて、倒立状態に復帰させるように動作制御することができる。【００９１】  このように、倒立停止制御（ステップＳ１３）、倒立走行制御（ステップＳ２１）、速度制御（ステップＳ３２）とを適宜切替ながら、倒立二輪走行することによって、車体１に接触が発生しても、接触状態（すなわち車体拘束状態）に応じて、車体回転に寄与するトルクの割合を変化させることで、過大なトルクが車輪２にかかって姿勢制御を維持できず走行速度が急上昇するといった暴走を防止することができ、ユーザに負担をかけずに、安全に走行を継続することができる。【００９２】  （第３実施形態）
第２実施形態においては、実際の接触センサ１０ａ，１０ｂの入力によって接触の有無を検知して制御方法を切り替えたが、本発明の第３実施形態では、走行する地面８００が、平坦な面であるということがわかっている場合として、接触センサ無しで、車体１の傾斜角度により制御方法の切替を行うものであって、車体１の傾斜角度が所定角度以上傾いたときには切替制御を行うものである。すなわち、図１１のように、車体１が傾いて地面８００に必ず接触する場合の車体１の傾斜角が、
【数１２５】
2007013282.tif 000126
として、車体１が傾いて地面８００に接触しないとき、すなわち、
【数１２６】
2007013282.tif 000127
のときには、（式４９）で示す倒立走行制御を行って走行状態を維持する一方、車体１が傾いて地面８００に接触するとき、すなわち、
【数１２７】
2007013282.tif 000128
の時には、（式５４）に基づく制御を行う。【００９３】  ここで、一例として、図１１のように、倒立停止又は走行状態（一点鎖線で示すＩ）に対して、傾斜位置（点線で示すＩＩ又はＩＩＩ）のように１０°傾斜すると、車体１が地面８００に接触していると仮定するときには、０．５°の許容値を考慮して、９．５°以上傾くと、車体１が地面８００に接触していると判断するようにすればよい。すなわち、
【数１２８】
2007013282.tif 000129
又は、
【数１２９】
2007013282.tif 000130
のときは、車体１が地面８００に接触していると判断するようにすればよい。よって、車体１が傾いて地面８００に接触しないとき、すなわち、
【数１３０】
2007013282.tif 000131
のときには、（式４９）で示す倒立走行制御を行って走行状態を維持する一方、車体１が傾いて地面８００に接触するとき、すなわち、
【数１３１】
2007013282.tif 000132
の時には、（式５４）に基づく速度制御を行う。【００９４】  以上のように、車体１の傾斜角度に応じて制御方法を切り替えながら走行することによって、平坦な面において接触が発生しても、暴走することなく安全にかつユーザに負担をかけずに走行を継続できる。【００９５】  なお、上記様々な実施形態のうちの任意の実施形態を適宜組み合わせることにより、それぞれの有する効果を奏するようにすることができる。
【産業上の利用可能性】
【００９６】  本発明にかかる倒立二輪走行型ロボット及びその制御方法では、地面や壁などに接触して回転を拘束された場合でも、姿勢制御や走行状態を維持でき、かつユーザに負担をかけずに、安全に移動することができる倒立二輪走行型ロボット及びその制御方法を提供できる。【００９７】  本発明は、添付図面を参照しながら好ましい実施形態に関連して充分に記載されているが、この技術の熟練した人々にとっては種々の変形や修正は明白である。そのような変形や修正は、添付した請求の範囲による本発明の範囲から外れない限りにおいて、その中に含まれると理解されるべきである。
【図１Ａ】
2007013282.tif 000133
【図１Ｂ】
2007013282.tif 000134
【図２】
2007013282.tif 000135
【図３】
2007013282.tif 000136
【図４】
2007013282.tif 000137
【図５】
2007013282.tif 000138
【図６】
2007013282.tif 000139
【図７】
2007013282.tif 000140
【図８】
2007013282.tif 000141
【図９】
2007013282.tif 000142
【図１０】
2007013282.tif 000143
【図１１】
2007013282.tif 000144
【図１２】
2007013282.tif 000145
【図１３】
2007013282.tif 000146
【図１４】
2007013282.tif 000147
【手続補正書】
【提出日】平成18年9月13日(2006.9.13)
【手続補正１】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項１】
車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記第１状態検出手段又は上記第２状態検出手段で検出された情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを検出する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボット。
【請求項２】
上記車体に対する接触力を検出する少なくとも一つの接触センサを上記車体に有するとともに、上記車体拘束認識手段は、上記接触センサで検出された接触情報を入力として、車体拘束状態を認識する請求項１に記載の倒立二輪走行型ロボット。
【請求項３】
車体拘束時には、上記車輪を回転駆動させるトルクに、上記車体への拘束力による発生するトルク分を加算する制御方法に切り替える請求項１又は２に記載の倒立二輪走行型ロボット。
【請求項４】
車体拘束時には上記車体の傾斜角度及び傾斜角速度に対するフィードバックを停止させる制御に切り替える請求項１又は２に記載の倒立二輪走行型ロボット。
【請求項５】
車体拘束時には上記車体の傾斜角度及び傾斜角速度に対するフィードバックを停止させ、かつ、上記車体の走行のみを継続する制御に切り替える請求項４に記載の倒立二輪走行型ロボット。
【請求項６】
車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする請求項１又は２に記載の倒立二輪走行型ロボット。
【請求項７】
車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする請求項３に記載の倒立二輪走行型ロボット。
【請求項８】
車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする請求項４に記載の倒立二輪走行型ロボット。
【請求項９】
車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする請求項５に記載の倒立二輪走行型ロボット。
【請求項１０】
車体の傾斜角度及び傾斜角速度の少なくとも１つを第１状態検出情報として検出し、
上記車輪の回転角度及び回転角速度の少なくとも１つを第２状態検出情報として検出し、
上記第１状態検出情報又は上記第２状態検出情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを車体拘束認識情報として検出し、
上記検出された車体拘束認識情報に基づいて、制御手段が複数の走行制御方法を切替えて、上記車体に同軸に配置された２つの車輪を各々駆動する駆動装置への指令値を決定し、決定された上記指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボットの制御方法。
【手続補正２】
【補正対象書類名】明細書
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、荷物運搬作業を行うロボットのうち、同軸二輪車を用いた倒立二輪走行型ロボット及びその制御方法に関するものであり、詳しくは、ロボット姿勢拘束時の倒立二輪走行型ロボット及びその制御方法に関するものである。
【背景技術】
【０００２】
荷物運搬作業を行うロボットしては、三輪型や四輪型など３点以上で必ず接触して安定を保つものが一般的である。しかしながら、三輪型、四輪型などは、面積を有すること、小回りが難しいことから倒立振子モデルに基づいて動作する同軸二輪車型のロボットが研究されてきている。
【０００３】
同軸二輪車型の移動ロボットとしては、例えば特許文献１、特許文献２に記載のものが知られている。
【０００４】
図１３は、上記特許文献１に記載された従来の同軸二輪車型の移動ロボットを示すものである。
【０００５】
特許文献１に記載された同軸二輪車型移動ロボットは、両端に一対の車輪１０２，１０３を備えた車軸１０１上の回動可能に支持された車体１０４と、車体１０４に装着された車輪駆動用モータ１０５と、車輪駆動用モータ１０５に作動指令を送る制御コンピュータ１０６と、車体１０４の傾きを検出する角度検出手段１０７を備えている。そして、角度検出手段１０７により検出される車体１０４の傾斜角度を短時間間隔にてサンプリングし、車体１０４の傾斜角度を状態変数入力値、フィードバックゲインＫを係数として、制御コンピュータ１０６内に予め入力設定された制御入力算出式に基づき、車輪駆動用モータ１０５の制御トルクを算出する状態フィードバック制御である。この算出された制御トルク相当の作動を制御コンピュータ１０６から、車輪駆動用モータ１０５に指令することにより、倒立の姿勢制御が維持される。
【０００６】
図１４は、上記特許文献２に記載された人乗り型の同軸二輪車型移動ロボットを示すものである。
【０００７】
特許文献２に記載された人乗り型の同軸二輪車型移動ロボットは、プラットホーム２０１上に同軸に配された左右の駆動輪２０２を姿勢感知センサーの出力に応じて制御駆動することで前後方向のバランスの保持のための姿勢制御と走行制御とを行う。そして、駆動輪２０２の前方側又は及び後方側で接地する補助輪２０３と、補助輪２０３を出没させる補助輪駆動部２０４とを備えている。そして、障害物検知センサー２０５と走行速度及び姿勢の制御の状態に応じて補助輪駆動部２０４を駆動して補助輪２０３を出没させる制御回路を備えているため、転倒を招く原因となりやすい障害物があった時、補助輪２０３が出て安定性を高められるために、転倒の危険性を少なくすることができる。
【０００８】
【特許文献１】特許２５３０６５２号
【特許文献２】特開２００４−７４８１４
【発明の開示】
【発明が解決しようとする課題】
【０００９】
上述の特許文献１に示された技術においては、状態フィードバック制御を常に行っている。
【００１０】
そして、同軸二輪車の車体１０４と車輪１０２，１０３が共に拘束無く相対的に回転できる場合には、車輪駆動用モータ１０５で発生したトルクは、車輪１０２，１０３の回転に寄与すると同時に、反作用のトルクが車体１０４に働いて、全体として倒立姿勢制御を維持できる。
【００１１】
しかしながら、車体１０４の一部が壁や地面に接触し、それ以上回転できなくなるような拘束力を外部から受けると車体１０４が回転しないだけでなく、拘束力によるトルクが車輪１０２，１０３に伝達される。よって、拘束力がない場合に比べて、車輪１０２，１０３に対して過大なトルクがかかり、車輪回転が増し、走行速度が急上昇するという課題を有していた。
【００１２】
上述の特許文献２に示された技術においては、障害物検知センサー２０５で障害物を検知した場合、補助輪駆動部２０４を駆動して補助輪２０３を出没させる機構を有しているため、補助輪２０３が出没した場合には、左右の駆動輪２０２と補助輪２０３の３点接地になり、転倒の危険性を少なくできる。
【００１３】
しかし、本技術では障害物を認識して補助輪２０３を出すだけで、速度や姿勢は人の操縦に依存している構成となっている。本構成においても、人がバランスを崩してプラットホーム２０１を含めた本体部が地面に接触した場合や、プラットホーム２０１を含めた本体部が壁等に接触して回転できなくなるような拘束力を受けた場合、特許文献１の場合と同様に駆動輪２０２に過大なトルクがかかり、走行速度が急上昇するという課題を有していた。この場合は、人の操縦により停止指令を送るなどの、危険を回避することになるが、操作が人に依存するという点でユーザに負担をかけるという課題を有していた。
【００１４】
本発明の目的は、上記従来の課題を解決するもので、倒立制御を行いながら移動する同軸二輪車の車体の一部が地面や壁などに接触して拘束力を受けた場合に、姿勢制御や走行状態を維持可能でかつユーザに負担をかけない安全な倒立二輪走行型ロボット及びその制御方法を提供する。
【課題を解決するための手段】
【００１５】
上記目的を達成するために、本発明は以下のように構成する。
【００１６】
本発明の第１態様によれば、車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記第１状態検出手段又は上記第２状態検出手段で検出された情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを検出する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボットを提供する。
【００１７】
本発明の第１０態様によれば、車体の傾斜角度及び傾斜角速度の少なくとも１つを第１状態検出情報として検出し、
上記車輪の回転角度及び回転角速度の少なくとも１つを第２状態検出情報として検出し、
上記第１状態検出情報又は上記第２状態検出情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを車体拘束認識情報として検出し、
上記検出された車体拘束認識情報に基づいて、制御手段が複数の走行制御方法を切替えて、上記車体に同軸に配置された２つの車輪を各々駆動する駆動装置への指令値を決定し、決定された上記指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボットの制御方法を提供する。
【発明の効果】
【００１８】
本発明の構成によれば、倒立して姿勢制御を行いながら走行する倒立二輪走行型ロボットの車体の一部が、地面や壁などに接触して回転を拘束された場合に、車体拘束認識手段又は車体拘束認識動作によって車体の回転方向の拘束が認識される。そして、認識された拘束状態に応じて、例えば車体回転に寄与するトルクの割合を変化させることで過大なトルクが車輪にかかって姿勢制御を維持できず走行速度が急上昇するという課題を回避することができる。
【００１９】
よって、本発明の構成によれば、倒立制御を行いながら移動する同軸二輪車型の倒立二輪走行型ロボットにおいて、車体の一部が地面や壁などに接触して回転を拘束された場合でも、姿勢制御や走行状態を維持でき、ユーザに負担をかけずに、安全に移動することができる。
【発明を実施するための最良の形態】
【００２０】
本発明の記述を続ける前に、添付図面において同じ部品については同じ参照符号を付している。
【００２１】
以下に、本発明にかかる実施の形態を説明する前に、本発明の種々の態様について説明する。
【００２２】
本発明の第１態様によれば、車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記第１状態検出手段又は上記第２状態検出手段で検出された情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを検出する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボットを提供する。
【００２３】
本発明の第２態様によれば、上記車体に対する接触力を検出する少なくとも一つの接触センサを上記車体に有するとともに、上記車体拘束認識手段は、上記接触センサで検出された接触情報を入力として、車体拘束状態を認識する第１の態様に記載の倒立二輪走行型ロボットを提供する。
【００２４】
本発明の第３態様によれば、車体拘束時には、上記車輪を回転駆動させるトルクに、上記車体への拘束力による発生するトルク分を加算する制御方法に切り替える第１又は２の態様に記載の倒立二輪走行型ロボットを提供する。
【００２５】
本発明の第４態様によれば、車体拘束時には上記車体の傾斜角度及び傾斜角速度に対するフィードバックを停止させる制御に切り替える第１又は２の態様に記載の倒立二輪走行型ロボットを提供する。
【００２６】
本発明の第５態様によれば、車体拘束時には上記車体の傾斜角度及び傾斜角速度に対するフィードバックを停止させ、かつ、上記車体の走行のみを継続する制御に切り替える第４の態様に記載の倒立二輪走行型ロボットを提供する。
【００２７】
本発明の第６態様によれば、車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする第１又は２の態様に記載の倒立二輪走行型ロボットを提供する。
【００２８】
本発明の第７態様によれば、車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする第３の態様に記載の倒立二輪走行型ロボットを提供する。
【００２９】
本発明の第８態様によれば、車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする第４の態様に記載の倒立二輪走行型ロボットを提供する。
【００３０】
本発明の第９態様によれば、車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする第５の態様に記載の倒立二輪走行型ロボットを提供する。
【００３１】
本発明の第１０態様によれば、車体の傾斜角度及び傾斜角速度の少なくとも１つを第１状態検出情報として検出し、
上記車輪の回転角度及び回転角速度の少なくとも１つを第２状態検出情報として検出し、
上記第１状態検出情報又は上記第２状態検出情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを車体拘束認識情報として検出し、
上記検出された車体拘束認識情報に基づいて、制御手段が複数の走行制御方法を切替えて、上記車体に同軸に配置された２つの車輪を各々駆動する駆動装置への指令値を決定し、決定された上記指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボットの制御方法を提供する。
【００３２】
以下に、本発明にかかる実施の形態を図面に基づいて詳細に説明する。
【００３３】
（第１実施形態）
以下、本発明の第１実施形態における倒立二輪走行型ロボット及びその制御方法について、図面を参照しながら説明する。
【００３４】
本発明の第１実施形態における倒立二輪走行型ロボットの傾いた状態での側面図を図１Ａ、傾いた状態での正面図を図２に示す。ロボットの車体１に対し、概して同一軸上に２つの車輪２ａ，２ｂが配置される。ここで、車体１の図中矢印Ａで示した進行方向に対して左車輪２ａ、右車輪２ｂとする。なお、車体１の進行方向から見た正面図が図２である。車輪２ａ，２ｂと車体１との間に独立してそれぞれ駆動力を発生させる駆動装置の一例として機能するアクチュエータ３ａ，３ｂは車体１の車輪２ａ，２ｂの近傍に取付けられ、各車輪２ａ，２ｂに連結されている。なお、説明のためアクチュエータ３ａ，３ｂは一例としてモータであるとする。なお、モータ３ａ，３ｂには、減速機４ａ，４ｂが取り付けられて、上記駆動装置の一例を構成するようにしてもよい。また、モータ３ａ，３ｂには、上記車輪２ａ，２ｂの回転角度及び回転角速度の少なくとも１つを第２状態検出情報として検出する第２状態検出手段の一例として機能するエンコーダ５ａ，５ｂが取り付けられており、車体１と車輪２ａ，２ｂの回転角度を計測できる。回転角度を一定サンプリング時間（例えば、１ｍｓｅｃおき）に計測し、その差分をサンプリング時間で除算することで、回転角速度が求まり、これを第２状態検出情報として扱うことができる。
【００３５】
また、車体１の車軸方向を法線とする鉛直平面内における姿勢を検出する姿勢センサ６の一例である２つのセンサ、すなわち、車体１の傾斜角度及び傾斜角速度の少なくとも１つを第１状態検出情報として検出する第１状態検出手段の一例として機能する１軸ジャイロ６ａと２軸加速度センサ６ｂが車体１の上部に取り付けられている。１軸ジャイロ６ａは、車軸方向を法線とする鉛直平面に、車軸方向の傾斜角速度を第１状態検出情報として検出できるように車体１に取り付けられ、車体１の車軸方向を法線とする鉛直平面内における傾斜角速度（円弧６ａ１で図示）を検出することができる。また、２軸加速度センサ６ｂは、車軸方向を法線とする鉛直平面の所定の２方向（６ｂ１，６ｂ２）に対する加速度を検出できるように取り付けられ、重力方向（６ｂ３）を検出することを可能としている。第１状態検出情報としての車体１の姿勢角度（傾斜角度）は、１軸ジャイロ６ａの積分値と２軸加速度センサ６ｂからの値をそれぞれ、制御コンピュータ部９でハイパスフィルター、ローパスフィルターにかけ、それらを制御コンピュータ部９で合成して求める。
【００３６】
また、車体１の前後の下部には接触センサ１０ａ，１０ｂが設けられ、車体１の当該車体１の前後の下部に対する他の物体などからの接触力を、接触情報としてそれぞれ測定することができる。
【００３７】
車体１の内部には、モータ３ａ，３ｂを駆動するためのモータドライバ７ａ，７ｂ及びバッテリ８、制御手段及び車体拘束認識手段の一例して機能する制御コンピュータ部９が搭載されている。図１Ｂに示すように、制御コンピュータ部９は、Ａ／Ｄ変換器９ａとＤ／Ａ変換器９ｂとエンコーダカウンタ部９ｃと演算部９ｄとから構成され、エンコーダ５ａ，５ｂの値はエンコーダカウンタ部９ｃを通じて演算部９ｄに入力される。また、１軸ジャイロ６ａと２軸加速度センサ６ｂ、接触センサ１０ａ，１０ｂの値はＡ／Ｄ変換器９ａを通じて演算部９ｄに入力される。演算部９ｄでは、入力された接触センサ１０ａ，１０ｂからの値に基づき、上記車体１の傾斜方向への回転が拘束されているかどうかを検出して、車体拘束認識情報を作成することができる。この作成された車体拘束認識情報は、例えば、接触センサ１０ａ，１０ｂの入力により判断される車体１に対する拘束力の情報（第１実施形態参照）、接触センサ１０ａ，１０ｂの入力により判断される接触の有無の情報（第２実施形態参照）、車体１の傾斜角度情報（第３実施形態参照）などであって、後述するように、制御方法を切替えるときに使用することができる。制御コンピュータ部９の演算部９ｄは、演算部９ｄに入力された値より、姿勢制御及び走行制御の必要トルクを計算し、Ｄ／Ａ変換器９ｂより、トルク指令値をモータドライバ７ａ，７ｂに指令する。モータドライバ７ａ，７ｂは、指令値に基づいてモータ３ａ，３ｂを駆動する。なお、入力値から出力値への計算方法は、後述する。
【００３８】
次に、本構成での倒立二輪走行型ロボットの制御方法について図３で説明する。図３はロボットを鉛直上方から見た図である。本機構においては、車軸方向を法線とする鉛直平面（Ｂ−Ｂ’面）内での姿勢及びその鉛直平面内の水平方向速度に関する並進制御１１（姿勢制御＋前後移動制御）と旋回方向の動作に関する旋回制御１２の２つの制御に分けて考える。前者は、並進方向の
【数１】
2007013282.tif 000148
に追従するように、姿勢制御と走行制御を行って走行状態を維持するものである。後者は、旋回方向の
【数２】
2007013282.tif 000149
に追従するように制御するものである。後述するように、この２つの制御を合わせることで、移動制御（走行制御）を行って走行状態を維持することができる。
【００３９】
次に、この移動制御について、順に、説明を行う。
【００４０】
まず、並進制御１１について、機構パラメータを示す図４を用いて説明する。なお、車軸方向を法線とする鉛直平面内での動作であるので、２次元モデルとして扱う。図４のように、パラメータを以下のように設定する。車輪２ａ，２ｂ、モータ３ａ，３ｂ、減速機４ａ，４ｂ、エンコーダ５ａ，５ｂ、はそれぞれ１つとみなして、車輪２、モータ３、減速機４、エンコーダ５とする。また、車体１の重心を車体重心１３、車輪２の車軸を車軸１４、車輪２の重心を車輪重心１５とする。
【００４１】
Ｍｂ：ロボットの車体１の質量［ｋｇ］
Ｍｗ：車輪２の質量（２つの車輪の合計）［ｋｇ］、
Ｉｂ：ロボットの車体１の車体重心１３周りの慣性モーメント［ｋｇｍ２］
Ｉｗ：車輪２の車輪重心１５周りの慣性モーメント（２つの車輪の合計）［ｋｇｍ２］
Ｉｍ：モータ３の慣性モーメント［ｋｇｍ２］
ｒ：車輪２の半径［ｍ］
Ｌｇ：車軸１４からロボットの車体重心１３までの距離［ｍ］
【００４２】
【数３】
2007013282.tif 000150
：モータ３と減速機４と車軸２との間の粘性摩擦係数［Ｎｍ／（ｒａｄ／ｓ）］
【数４】
2007013282.tif 000151
：車輪２と地面８００との間の粘性摩擦係数［Ｎｍ／（ｒａｄ／ｓ）］
【数５】
2007013282.tif 000152
：モータ３のトルク定数［Ｎｍ／Ａ］
【数６】
2007013282.tif 000153
：減速機４のギア部減速比
ｇ：重力加速度
【数７】
2007013282.tif 000154
：ロボットの車体１の傾き［ｒａｄ］
【数８】
2007013282.tif 000155
：車輪２の地面８００に対する回転角［ｒａｄ］
ｕ：モータ３に入力する電流［Ａ］
【００４３】
また、記号を次のように定義する。
Ｔ：運動エネルギー、Ｕ：位置エネルギー，Ｄ：摩擦損失
ｚ１：車体重心高さ，ｚ２：車輪重心高さ，ｓ１：車体重心並進位置，
ｓ２：車軸並進位置，
ここで、倒立２輪走行型ロボットの運動方程式を、ラグランジェ法を用いて導出する。
ラグランジェの運動方程式は
【数９】
2007013282.tif 000156
【数１０】
2007013282.tif 000157
各値とその微分値は次のように表される。
【数１１】
2007013282.tif 000158
【数１２】
2007013282.tif 000159
【数１３】
2007013282.tif 000160
【数１４】
2007013282.tif 000161
【数１５】
2007013282.tif 000162
【数１６】
2007013282.tif 000163
【数１７】
2007013282.tif 000164
【数１８】
2007013282.tif 000165
各エネルギーは
【数１９】
2007013282.tif 000166
【数２０】
2007013282.tif 000167
【数２１】
2007013282.tif 000168
簡単なモデルとして、車体１にかかる外力が０とした場合、
【数２２】
2007013282.tif 000169
【数２３】
2007013282.tif 000170
よって、運動エネルギーＴは
【数２４】
2007013282.tif 000171
となる。
【００４４】
ロボットの車体１の傾きφについて、ラグランジェ法により運動方程式を導出する。
【数２５】
2007013282.tif 000172
【００４５】
車輪２の地面８００に対する回転角θについて、ラグランジェ法により運動方程式を導出する。
【数２６】
2007013282.tif 000173
（式１７）と（式１８）を足し合わせると、
【数２７】
2007013282.tif 000174
となる。
【００４６】
よって、２つの式（式１７）と（式１９）を
【数２８】
2007013282.tif 000175
の周りで近似して
【数２９】
2007013282.tif 000176
【数３０】
2007013282.tif 000177
となる。
【００４７】
ここで変数を
【数３１】
2007013282.tif 000178
【数３２】
2007013282.tif 000179
とおいて、
【数３３】
2007013282.tif 000180
【数３４】
2007013282.tif 000181
の状態方程式の形に帰着できる。なお、
【数３５】
2007013282.tif 000182
は、状態ベクトルである。
【００４８】
以上のモデル化は、状態ベクトルの各値が０での近似モデルであるので、前後方向の位置（速度指令）に関する追従性を考慮するために、このモデルに対してステップ状の目標入力に追従するモデルの制御系を構成する。なお、目標入力を一定速度目標値であるとする。
【数３６】
2007013282.tif 000183
は、可観測であるとし、
【数３７】
2007013282.tif 000184
と表せる。
【００４９】
【数３８】
2007013282.tif 000185
をステップ状の
【数３９】
2007013282.tif 000186
に定常偏差なく追従させるために、積分器を用いた制御系とする。なお、そのときの制御ブロックを図５に示す。ここで、
【数４０】
2007013282.tif 000187
の閉ループシステムを安定にするためのフィードバックベクトルである。
【００５０】
この構成において、
【数４１】
2007013282.tif 000188
より
【数４２】
2007013282.tif 000189
制御入力は、
【数４３】
2007013282.tif 000190
と表せる。ここで、Ｉｎは３行３列の単位行列、Ｉｍ＝１として
【数４４】
2007013282.tif 000191
（式２６）と（式３０）より
【数４５】
2007013282.tif 000192
【数４６】
2007013282.tif 000193
ここで、定常値は、
【数４７】
2007013282.tif 000194
【数４８】
2007013282.tif 000195
なお、
【数４９】
2007013282.tif 000196
となり、定常偏差がないことがわかる。
【００５１】
次に、定常値からの誤差システムを考える。
【数５０】
2007013282.tif 000197
として式変形を行うと
【数５１】
2007013282.tif 000198
となる。
【００５２】
【数５２】
2007013282.tif 000199
【数５３】
2007013282.tif 000200
として、
【数５４】
2007013282.tif 000201
となり、出力をｅとして
【数５５】
2007013282.tif 000202
となる。（式３９）、（式４０）にｗの状態フィードバックをしたレギュレータとみなせる。よって、この誤差システムのフィードバックゲインＨを、最適レギュレータ法や極配置法、その他の種々の設計法によって導出する。このゲインを用いれば、
【数５６】
2007013282.tif 000203
の過渡応答誤差の少ない最適なサーボシステムが設計できる。なお、（式３７）より
【数５７】
2007013282.tif 000204
となる。したがって、
【数５８】
2007013282.tif 000205
を用いて、モータ３へ出力される
【数５９】
2007013282.tif 000206
を、
【数６０】
2007013282.tif 000207
とすれば、倒立の姿勢制御と速度制御が同時に実現できる。
【００５３】
ただし、
【数６１】
2007013282.tif 000208
【数６２】
2007013282.tif 000209
である。なお、
【数６３】
2007013282.tif 000210
である。本構成では、車輪２は２つであるので、
【数６４】
2007013282.tif 000211
の０．５倍を各車輪２（２ａ，２ｂ）に出力すれば、並進方向の姿勢制御と速度制御を行える。そのときの、各車輪２ａ，２ｂの
【数６５】
2007013282.tif 000212
として、
【数６６】
2007013282.tif 000213
となる。
【００５４】
次に、旋回制御１２について旋回制御模式図である図６を用いて説明する。
【００５５】
ロボットの車体１を上面から見て、ロボットの車体１の車輪２間の長さを
【数６７】
2007013282.tif 000214
とし、水平面上でのロボットの車体１の移動速度を
【数６８】
2007013282.tif 000215
とし、左車輪２ａの回転速度を
【数６９】
2007013282.tif 000216
とし、右車輪２ｂの回転速度を
【数７０】
2007013282.tif 000217
とし、旋回半径をＲ、旋回速度を
【数７１】
2007013282.tif 000218
とする。また、左車輪２ａの回転角速度を
【数７２】
2007013282.tif 000219
、右車輪２ｂの回転角速度を
【数７３】
2007013282.tif 000220
とすると、
【数７４】
2007013282.tif 000221
となる。
【００５６】
【数７５】
2007013282.tif 000222
として、
【数７６】
2007013282.tif 000223
との差に対してフィードバック（ＰＤ制御等）を行い、
【数７７】
2007013282.tif 000224
を決定する。
【００５７】
【数７８】
2007013282.tif 000225
は、左右の車輪２ａ，２ｂに発生するトルク差であるとみなせる。左右の車輪２ａ，２ｂに、それぞれ、その旋回方向のトルクの０．５倍を、旋回方向の目標トルクを算出できる両車輪２ａ，２ｂに、それぞれ加算、減算してやることで、旋回方向の制御が実現できる。
【００５８】
並進制御１１、旋回制御１２の統合された制御ブロック図を図７に示す。
【００５９】
そして、左輪２ａ、右輪２ｂに指令するトルク指令値を
【数７９】
2007013282.tif 000226
とすると
【数８０】
2007013282.tif 000227
【００６０】
以上のように、トルクを決定することにより、上記ロボットは、倒立姿勢を維持しながら旋回及び直線走行をすることができる。
【００６１】
図１Ａのように、車体１が地面８００から離れている場合には、これまで述べたような制御則に基づけば、倒立姿勢を維持しながらの走行を行うことができる。
【００６２】
次に、第１実施形態では、図８のように車体１の一部が回転方向に拘束を受けた場合の一例として、車体１が傾いて車体下部が地面８００に接触した場合を示す。仮定として、車体１と地面８００との摩擦力は０で
【数８１】
2007013282.tif 000228
だけが、車軸１４から水平移動した
【数８２】
2007013282.tif 000229
の接触点において拘束力として発生するとする。接触センサ１０ｂ（接触センサ１０ａも同様であるため、代表的に接触センサ１０ｂで説明する。）は、車体１に対する他の物体や地面８００や壁などからの接触力を計測できるセンサとし、接触力センサ１０ｂによって
【数８３】
2007013282.tif 000230
が検出され、幾何学的な条件より
【数８４】
2007013282.tif 000231
も既知である。このとき、車体１には拘束力による
【数８５】
2007013282.tif 000232
が発生する。また、位置ずれ量が
【数８６】
2007013282.tif 000233
の状態のときに、図８のような
【数８７】
2007013282.tif 000234
接触が発生した場合を考える。このとき、（式４２）に基づくモータ３のトルク指令値は、車輪２を正転させる方向（ロボットの車体１が図８において右方向に移動し、車体１が車軸１４周りに左回転する方向）を正として、
【数８８】
2007013282.tif 000235
となる。
【００６３】
【数８９】
2007013282.tif 000236
であるので、傾斜方向の回転が拘束され、
【数９０】
2007013282.tif 000237
となる。
このとき
【数９１】
2007013282.tif 000238
となる。
【００６４】
【数９２】
2007013282.tif 000239
は、モータ３からのトルクの
【数９３】
2007013282.tif 000240
と、
【数９４】
2007013282.tif 000241
の和になるため、
【数９５】
2007013282.tif 000242
となる。この
【数９６】
2007013282.tif 000243
の値によっては、
【数９７】
2007013282.tif 000244
のいずれにもなりうる。
【００６５】
（式５１）において
【数９８】
2007013282.tif 000245
である場合には、車体１には、右回転させるようにするトルクが働くため、車軸１４周りに右回転し、接触は解消され倒立状態に戻る事ができる。しかしながら、
【数９９】
2007013282.tif 000246
である場合には、車体１には、左回転のトルクがかかり、車輪２には、反力として右へ移動する回転トルクが発生する。この時、車体１が
【数１００】
2007013282.tif 000247
で拘束されているので、車輪２には、
【数１０１】
2007013282.tif 000248
によって発生した
【数１０２】
2007013282.tif 000249
も、車輪２への回転トルクとして伝達される。そのため、車輪２に余分のトルクがかかり、車輪２の過度の回転現象を生じ、暴走現象に陥ってしまう。
【００６６】
そこで、本発明の上記第１実施形態にかかるロボットの制御方法では、接触力センサ１０ｂより接触情報を検知し、検知した接触力方向と
【数１０３】
2007013282.tif 000250
とを比較し、その回転方向が反対の場合には、モータ３への
【数１０４】
2007013282.tif 000251
を、接触力により発生する
【数１０５】
2007013282.tif 000252
を加算した
【数１０６】
2007013282.tif 000253
とする。
【００６７】
したがって、本発明による上記第１実施形態にかかるロボットの制御方法によれば、（式５２）に基づきモータ３のトルクを算出することにより、地面８００からの接触力による影響を除去でき、ユーザに負担をかけずに、安全な倒立二輪走行型ロボットを実現できる。
【００６８】
また、本構成において車体１と地面８００との間に
【数１０７】
2007013282.tif 000254
が発生する場合、図９のように、その
【数１０８】
2007013282.tif 000255
によって発生する回転の反対方向のトルクを補償して走行することとすれば同様の効果を得られる
【数１０９】
2007013282.tif 000256
また、外乱オブザーバを用いて、接触センサ無しに外乱力を推定してその推定された接触力を用いて（式５２）、（式５３）と同様の制御を行ってもよい。
【００６９】
ここで、上記した第１実施形態での切替制御を用いた走行動作フローチャートを図１２で説明する。
【００７０】
初期状態（ステップＳ１）では、ロボットの車体１は傾斜して停止している。初期設定として、
【数１１０】
2007013282.tif 000257
（＝ステップＳ２）とする。次に、（式５０）で示す倒立停止
【数１１１】
2007013282.tif 000258
制御（ステップＳ３）を行って起き上がる。倒立停止制御（ステップＳ３）の安定時間（例えば５秒程度）後（ステップＳ４）、車体１の地面８００などに対する接触の有無を判別する（ステップＳ５）。
【００７１】
ステップＳ５で接触が無い場合には、
【数１１２】
2007013282.tif 000259
設定を行った（ステップＳ８）後、（式４９）で示す倒立走行制御（ステップＳ９）を行って走行状態を維持した後、ステップＳ５に戻る。
【００７２】
ステップＳ５で接触がある場合には、
【数１１３】
2007013282.tif 000260
設定を行った（ステップＳ６）後、（式５２又は式５３）で示す倒立走行制御（ステップＳ７）を行って走行状態を維持した後、ステップＳ５に戻る。
【００７３】
なお、ステップＳ６とステップＳ８において、接地しているときも、接地していないときも、その差分トルクを補正するので、どちらの場合も、同様の走行が行える。
【００７４】
以上のように、上記第１実施形態によれば、制御コンピュータ部９の制御の下で、以下のような動作制御を行うことができる。
【００７５】
すなわち、初期の車体１が移動停止して地面８００に接地している状態から、まず、倒立停止制御（式（４９）、ステップＳ２〜Ｓ３）を行うことにより、車体１を倒立位置まで起き上がらせて、倒立状態（ただし、走行は停止している状態。）を維持する。次いで、所定時間（安定時間）経過して倒立状態が安定した（ステップＳ４）のち、接触センサ１０ａ，１０ｂによる接触の有無の検出を開始する（ステップＳ５）。ステップＳ５で接触（言い換えれば、車体拘束）が無ければ、倒立停止制御から倒立走行制御（式（４９）、ステップＳ８〜Ｓ９）に移って走行状態を維持したのち、接触センサ１０ａ，１０ｂによる接触の有無の検出を再度行う（ステップＳ５）。ステップＳ５で接触（言い換えれば、車体拘束）が検出された場合には、倒立停止制御から倒立走行制御（式（５２）又は式（５３）、ステップＳ６〜Ｓ７）に移って走行状態を維持したのち、接触センサ１０ａ，１０ｂによる接触の有無の検出を再度行う（ステップＳ５）。
【００７６】
このように、倒立停止制御（ステップＳ３）、倒立走行制御（ステップＳ９）、倒立走行制御（ステップＳ７）とを適宜切替ながら、倒立二輪走行することによって、車体１に接触が発生しても、接触状態（すなわち車体拘束状態）に応じて、車体回転に寄与するトルクの割合を変化させることで、過大なトルクが車輪２にかかって姿勢制御を維持できず走行速度が急上昇するといった暴走を防止することができ、ユーザに負担をかけずに、安全に走行を継続することができる。
【００７７】
（第２実施形態）
本発明の第２実施形態を第１実施形態と同じ、図８で説明する。第１実施形態においては、接触センサを設けて接触力を検知していたが、第２実施形態では、接触センサ１０ｂが、接触の有無を検知のみで力測定ができないセンサの場合や、力計測できるセンサであっても接触の有無のみを利用する場合とし、その制御方法を述べる。
【００７８】
車体１の一部が地面８００などに接触している場合、車体１は回転できず、車体１は２つの車輪２ａ，２ｂと、少なくとも１点との３点以上で地面８００に接していることとなる。従来からあるような独立二輪駆動で、補助輪等で接地している台車の移動制御においては、車輪２の回転速度に対して、ＰＩＤ制御等によって走行制御がなされて走行状態を維持している。そのため、この第２実施形態においても、接地時には同様の構成とみなすことができるので、接地した場合には、
【数１１４】
2007013282.tif 000261
に示すような、回転速度に対する略ＰＩ制御の速度制御の構成とする。したがって、接触がある場合には、（式５４）、接触がない場合には（式５０）の制御を切り替えて制御を行う。
【００７９】
第２実施形態での切替制御を用いた走行動作フローチャートを図１０で説明する。
【００８０】
初期状態（ステップＳ１１）では、ロボットの車体１は傾斜して停止している。初期設定として、
【数１１５】
2007013282.tif 000262
（＝ステップＳ１２）とする。次に、（式５０）で示す倒立停止
【数１１６】
2007013282.tif 000263
制御（ステップＳ１３）を行って起き上がる。倒立停止制御（ステップＳ１３）の安定時間（例えば５秒程度）後（ステップＳ１４）、接触の有無を判別する（ステップＳ１５）。もし接触（ステップＳ１５）のない場合は、
【数１１７】
2007013282.tif 000264
設定を行った（ステップＳ２０）後、（式４９）で示す倒立走行制御（ステップＳ２１）を行って走行状態を維持している。（式４９）に基づく倒立走行制御（ステップＳ２１）から、接触時の（式５４）に基づく速度制御（ステップＳ１８）へと制御方法が移る条件として、積分項リセット履歴ビットｂｔ１を設ける。積分項とは、速度差の
【数１１８】
2007013282.tif 000265
（車体１の現在位置に対して車体１のバランスが崩れたりなどして位置ずれが生じるときの位置ずれ量）である。そして、倒立走行制御（ステップＳ２１）中に、接触の有無を判断する（ステップＳ２２）。
【００８１】
ステップＳ２２で車体１が地面８００などに接触していない場合には、積分項リセット履歴ビットｂｔ１をリセット（ｂｔ１＝０）した（ステップＳ２６）後（すなわち、積分項のリセットは行わずに）、ステップＳ２０に戻る。
【００８２】
ステップＳ２２で車体１が地面８００などに接触している場合には、積分項リセット履歴がＯＮか否か、すなわち、積分項リセット履歴ビットｂｔ１＝１か否かを判断する（ステップＳ２３）。積分項リセット履歴ビットｂｔ１＝０のとき（接触後に積分項のリセットを行っていなかったとき）のみ、
【数１１９】
2007013282.tif 000266
を
【数１２０】
2007013282.tif 000267
した（ステップＳ２７）後（積分項をリセットし、かつ、積分項をリセットしたことを示す履歴情報を残した後）、ステップＳ２０に戻る。積分項をリセットするのは、接触する前に発生している位置ずれ（速度差の
【数１２１】
2007013282.tif 000268
）を、一度、０にして、接触時からのＰＩ制御にするためである。このように、積分項をリセットすることによって、接触前の位置ずれ量を削除できて、接触後の車体１の位置ずれ量のみにより、動作制御が行われるため、車体１の暴走現象を防止することができる。
ステップＳ２３で積分項リセット履歴ビットｂｔ１＝１であるとき、接触の有無を判別する（ステップＳ２４）。
【００８３】
ステップＳ２４で車体１が地面８００などに接触していない場合は、積分項リセット履歴ビットｂｔ１をリセット（ｂｔ１＝０）した（ステップＳ２８）後（すなわち、積分項のリセットは行わずに）、ステップＳ２０に戻る。
【００８４】
ステップＳ２４で、そのまま、車体１が地面８００などに接触が続いている場合（ｂｔ１＝１）は、安定時間（例えば１秒）（ステップＳ２５）以内であれば、ステップＳ２０に戻り、倒立走行（ステップＳ２１）を継続する。
【００８５】
一方、安定時間（例えば１秒）が経過していれば、倒立走行（ステップＳ２１）から速度制御（ステップＳ３０〜Ｓ３２）に移る。
【００８６】
ステップＳ１５で車体１が地面８００などに接触している場合又はステップＳ２５で安定時間経過時には、
【数１２２】
2007013282.tif 000269
設定を行った（ステップＳ３０）後、
【数１２３】
2007013282.tif 000270
か否かを判断する（ステップＳ３１）。ＹＥＳの場合には、起き上がれないと判断して終了する（ステップＳ１９）。ステップＳ３１で、ＮＯ、すなわち、
【数１２４】
2007013282.tif 000271
の場合には、（式５４）で示す速度制御（ステップＳ３２）で走行したのち、ステップＳ１４に戻り、速度制御（ステップＳ３２）の安定時間（例えば５秒程度）が経過したか否かを判断する（ステップＳ１４）。
【００８７】
以上のように、上記第２実施形態によれば、制御コンピュータ部９の制御の下で、以下のような動作制御を行うことができる。
【００８８】
すなわち、初期の車体１が移動停止して地面８００に接地している状態から、まず、倒立停止制御（式（４９）、ステップＳ１２〜Ｓ１３）を行うことにより、車体１を倒立位置まで起き上がらせて、倒立状態（ただし、走行は停止している状態。）を維持する。次いで、所定時間（安定時間）経過して倒立状態が安定した（ステップＳ１４）のち、接触センサ１０ａ，１０ｂによる接触の有無の検出を開始する（ステップＳ１５）。ステップＳ１５で接触（言い換えれば、車体拘束）が無ければ、倒立停止制御から倒立走行制御（式（４９）、ステップＳ２０〜Ｓ２１）に移って走行状態を維持する。ステップＳ１５で接触（言い換えれば、車体拘束）が検出された場合には、倒立停止制御から速度制御（ステップＳ３０〜Ｓ３２）に移る。次いで、所定時間（安定時間）経過して安定して速度制御が行われた（ステップＳ１４）のち、接触センサ１０ａ，１０ｂによる接触の有無の検出を再度行い（ステップＳ１５）、ステップＳ１５で接触がなければ（起き上がりが成功したならば）、倒立走行制御（式（４９）、ステップＳ２０〜Ｓ２１）に移って走行状態を維持する。ステップＳ１５で接触があれば（起き上がりが不成功ならば）、そのまま、速度制御（ステップＳ３０〜Ｓ３２）を継続する。速度制御（ステップＳ３０〜Ｓ３２）を行うとき、速度及び角速度が０の場合には、車体１が物体又は地面８００などに接触している場合など、車体１が起き上がれない場合であると判断して、車体１の起き上がり駆動を停止し、接地状態とする。
【００８９】
また、倒立走行制御（式（４９）、ステップＳ２０〜Ｓ２１）中に接触があった場合（ステップＳ２２）には、接触時までの位置ずれ量をリセットして０にしたのち、接触時からの位置ずれ量のみにより、動作制御が行われるため、車体１の暴走現象を防止することができる。さらに、倒立走行制御時に、接触時までの位置ずれ量をリセットして０にしたのち、接触が安定時間経過しても続いている場合には、車体１の倒立状態が崩れていると判断して、倒立走行制御から速度制御に切り替えて、倒立状態に復帰させるように動作制御することができる。
【００９０】
このように、倒立停止制御（ステップＳ１３）、倒立走行制御（ステップＳ２１）、速度制御（ステップＳ３２）とを適宜切替ながら、倒立二輪走行することによって、車体１に接触が発生しても、接触状態（すなわち車体拘束状態）に応じて、車体回転に寄与するトルクの割合を変化させることで、過大なトルクが車輪２にかかって姿勢制御を維持できず走行速度が急上昇するといった暴走を防止することができ、ユーザに負担をかけずに、安全に走行を継続することができる。
【００９１】
（第３実施形態）
第２実施形態においては、実際の接触センサ１０ａ，１０ｂの入力によって接触の有無を検知して制御方法を切り替えたが、本発明の第３実施形態では、走行する地面８００が、平坦な面であるということがわかっている場合として、接触センサ無しで、車体１の傾斜角度により制御方法の切替を行うものであって、車体１の傾斜角度が所定角度以上傾いたときには切替制御を行うものである。すなわち、図１１のように、車体１が傾いて地面８００に必ず接触する場合の車体１の傾斜角が、
【数１２５】
2007013282.tif 000272
として、車体１が傾いて地面８００に接触しないとき、すなわち、
【数１２６】
2007013282.tif 000273
のときには、（式４９）で示す倒立走行制御を行って走行状態を維持する一方、車体１が傾いて地面８００に接触するとき、すなわち、
【数１２７】
2007013282.tif 000274
の時には、（式５４）に基づく制御を行う。
【００９２】
ここで、一例として、図１１のように、倒立停止又は走行状態（一点鎖線で示すＩ）に対して、傾斜位置（点線で示すＩＩ又はＩＩＩ）のように１０°傾斜すると、車体１が地面８００に接触していると仮定するときには、０．５°の許容値を考慮して、９．５°以上傾くと、車体１が地面８００に接触していると判断するようにすればよい。すなわち、
【数１２８】
2007013282.tif 000275
又は、
【数１２９】
2007013282.tif 000276
のときは、車体１が地面８００に接触していると判断するようにすればよい。よって、車体１が傾いて地面８００に接触しないとき、すなわち、
【数１３０】
2007013282.tif 000277
のときには、（式４９）で示す倒立走行制御を行って走行状態を維持する一方、車体１が傾いて地面８００に接触するとき、すなわち、
【数１３１】
2007013282.tif 000278
の時には、（式５４）に基づく速度制御を行う。
【００９３】
以上のように、車体１の傾斜角度に応じて制御方法を切り替えながら走行することによって、平坦な面において接触が発生しても、暴走することなく安全にかつユーザに負担をかけずに走行を継続できる。
【００９４】
なお、上記様々な実施形態のうちの任意の実施形態を適宜組み合わせることにより、それぞれの有する効果を奏するようにすることができる。
【産業上の利用可能性】
【００９５】
本発明にかかる倒立二輪走行型ロボット及びその制御方法では、地面や壁などに接触して回転を拘束された場合でも、姿勢制御や走行状態を維持でき、かつユーザに負担をかけずに、安全に移動することができる倒立二輪走行型ロボット及びその制御方法を提供できる。
【００９６】
本発明は、添付図面を参照しながら好ましい実施形態に関連して充分に記載されているが、この技術の熟練した人々にとっては種々の変形や修正は明白である。そのような変形や修正は、添付した請求の範囲による本発明の範囲から外れない限りにおいて、その中に含まれると理解されるべきである。
【図面の簡単な説明】
【００９７】
本発明のこれらと他の目的と特徴は、添付された図面についての好ましい実施形態に関連した次の記述から明らかになる。
【図１Ａ】図１Ａは、本発明の第１実施形態における倒立二輪走行型ロボットの傾いた状態での側面図である。
【図１Ｂ】図１Ｂは、本発明の第１実施形態における倒立二輪走行型ロボットの制御部などのブロック図である。
【図２】図２は、本発明の第１実施形態における倒立二輪走行型ロボットの傾いた状態での正面図である。
【図３】図３は、本発明の第１実施形態における倒立二輪走行型ロボットの移動構成の説明図である。
【図４】図４は、本発明の第１実施形態における倒立二輪走行型ロボットの機構パラメータ図である。
【図５】図５は、本発明の第１実施形態における倒立二輪走行型ロボットの制御ブロック図である。
【図６】図６は、本発明の第１実施形態における倒立二輪走行型ロボットの旋回制御模式図である。
【図７】図７は、本発明の第１実施形態における走行制御全体図である。
【図８】図８は、本発明の第２実施形態における倒立二輪走行型ロボットのフローチャートである。
【図９】図９は、本発明の第１実施形態における倒立二輪走行型ロボットの接触状態図である。
【図１０】図１０は、本発明の第２実施形態における倒立二輪走行型ロボットのフローチャートである。
【図１１】図１１は、本発明の第３実施形態における倒立二輪走行型ロボットの車体が傾いて地面に必ず接触する場合の説明図である。
【図１２】図１２は、本発明の第１実施形態における倒立二輪走行型ロボットのフローチャートである。
【図１３】図１３は、従来の特許文献１に記載の同軸二輪車型の移動ロボットの図である。
【図１４】図１４は、従来の特許文献２に記載の人乗り型の同軸二輪車型移動ロボットの図である。
【手続補正３】
【補正対象書類名】図面
【補正対象項目名】全図
【補正方法】変更
【補正の内容】
【図１Ａ】
2007013282.tif 000279
【図１Ｂ】
2007013282.tif 000280
【図２】
2007013282.tif 000281
【図３】
2007013282.tif 000282
【図４】
2007013282.tif 000283
【図５】
2007013282.tif 000284
【図６】
2007013282.tif 000285
【図７】
2007013282.tif 000286
【図８】
2007013282.tif 000287
【図９】
2007013282.tif 000288
【図１０】
2007013282.tif 000289
【図１１】
2007013282.tif 000290
【図１２】
2007013282.tif 000291
【図１３】
2007013282.tif 000292
【図１４】
2007013282.tif 000293
【手続補正書】
【提出日】平成18年9月14日(2006.9.14)
【手続補正１】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項１】
車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記第１状態検出手段又は上記第２状態検出手段で検出された情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを検出する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記車体に対する接触力を検出する少なくとも一つの接触センサを上記車体に有するとともに、上記車体拘束認識手段は、上記接触センサで検出された接触情報を入力として、車体拘束状態を認識し、
上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する倒立二輪走行型ロボット。
【請求項２】
車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記第１状態検出手段又は上記第２状態検出手段で検出された情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを検出する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する一方、車体拘束時には、上記車輪を回転駆動させるトルクに、上記車体への拘束力による発生するトルク分を加算する制御方法に切り替える倒立二輪走行型ロボット。
【請求項３】
車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記第１状態検出手段又は上記第２状態検出手段で検出された情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを検出する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記車体に対する接触力を検出する少なくとも一つの接触センサを上記車体に有するとともに、上記車体拘束認識手段は、上記接触センサで検出された接触情報を入力として、車体拘束状態を認識し、
上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する一方、車体拘束時には、上記車輪を回転駆動させるトルクに、上記車体への拘束力による発生するトルク分を加算する制御方法に切り替える倒立二輪走行型ロボット。
【請求項４】
車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする請求項１に記載の倒立二輪走行型ロボット。
【請求項５】
車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする請求項２又は３に記載の倒立二輪走行型ロボット。
【請求項６】
車体拘束時には上記車体の傾斜角度及び傾斜角速度に対するフィードバックを停止させる制御に切り替えるとともに、上記車輪の回転角速度の誤差の積分値をリセットする請求項１又は２又は３に記載の倒立二輪走行型ロボット。
【請求項７】
車体拘束時には上記車体の傾斜角度及び傾斜角速度に対するフィードバックを停止させ、かつ、上記車体の走行のみを継続する制御に切り替えるとともに、上記車輪の回転角速度の誤差の積分値をリセットする請求項１又は２又は３に記載の倒立二輪走行型ロボット。
【手続補正２】
【補正対象書類名】明細書
【補正対象項目名】００１６
【補正方法】変更
【補正の内容】
【００１６】
本発明の第１態様によれば、車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記第１状態検出手段又は上記第２状態検出手段で検出された情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを検出する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記車体に対する接触力を検出する少なくとも一つの接触センサを上記車体に有するとともに、上記車体拘束認識手段は、上記接触センサで検出された接触情報を入力として、車体拘束状態を認識し、
上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボットを提供する。
本発明の第２態様によれば、車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記第１状態検出手段又は上記第２状態検出手段で検出された情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを検出する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する一方、車体拘束時には、上記車輪を回転駆動させるトルクに、上記車体への拘束力による発生するトルク分を加算する制御方法に切り替える倒立二輪走行型ロボットを提供する。
本発明の第３態様によれば、車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記第１状態検出手段又は上記第２状態検出手段で検出された情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを検出する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記車体に対する接触力を検出する少なくとも一つの接触センサを上記車体に有するとともに、上記車体拘束認識手段は、上記接触センサで検出された接触情報を入力として、車体拘束状態を認識し、
上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する一方、車体拘束時には、上記車輪を回転駆動させるトルクに、上記車体への拘束力による発生するトルク分を加算する制御方法に切り替える倒立二輪走行型ロボットを提供する。
【手続補正３】
【補正対象書類名】明細書
【補正対象項目名】００１７
【補正方法】削除
【補正の内容】
【手続補正４】
【補正対象書類名】明細書
【補正対象項目名】００２２
【補正方法】変更
【補正の内容】
【００２２】
本発明の第１態様によれば、車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記第１状態検出手段又は上記第２状態検出手段で検出された情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを検出する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記車体に対する接触力を検出する少なくとも一つの接触センサを上記車体に有するとともに、上記車体拘束認識手段は、上記接触センサで検出された接触情報を入力として、車体拘束状態を認識し、
上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボットを提供する。
【手続補正５】
【補正対象書類名】明細書
【補正対象項目名】００２３
【補正方法】変更
【補正の内容】
【００２３】
本発明の第２態様によれば、車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記第１状態検出手段又は上記第２状態検出手段で検出された情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを検出する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する一方、車体拘束時には、上記車輪を回転駆動させるトルクに、上記車体への拘束力による発生するトルク分を加算する制御方法に切り替える倒立二輪走行型ロボットを提供する。
【手続補正６】
【補正対象書類名】明細書
【補正対象項目名】００２４
【補正方法】変更
【補正の内容】
【００２４】
本発明の第３態様によれば、車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記第１状態検出手段又は上記第２状態検出手段で検出された情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを検出する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記車体に対する接触力を検出する少なくとも一つの接触センサを上記車体に有するとともに、上記車体拘束認識手段は、上記接触センサで検出された接触情報を入力として、車体拘束状態を認識し、
上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する一方、車体拘束時には、上記車輪を回転駆動させるトルクに、上記車体への拘束力による発生するトルク分を加算する制御方法に切り替える倒立二輪走行型ロボットを提供する。
【手続補正７】
【補正対象書類名】明細書
【補正対象項目名】００２５
【補正方法】削除
【補正の内容】
【手続補正８】
【補正対象書類名】明細書
【補正対象項目名】００２６
【補正方法】削除
【補正の内容】
【手続補正９】
【補正対象書類名】明細書
【補正対象項目名】００２７
【補正方法】変更
【補正の内容】
【００２７】
本発明の第４態様によれば、車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする第１の態様に記載の倒立二輪走行型ロボットを提供する。
【手続補正１０】
【補正対象書類名】明細書
【補正対象項目名】００２８
【補正方法】変更
【補正の内容】
【００２８】
本発明の第５態様によれば、車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする第２又は３の態様に記載の倒立二輪走行型ロボットを提供する。
【手続補正１１】
【補正対象書類名】明細書
【補正対象項目名】００２９
【補正方法】変更
【補正の内容】
【００２９】
本発明の第６態様によれば、車体拘束時には上記車体の傾斜角度及び傾斜角速度に対するフィードバックを停止させる制御に切り替えるとともに、上記車輪の回転角速度の誤差の積分値をリセットする第１又は２又は３の態様に記載の倒立二輪走行型ロボットを提供する。
【手続補正１２】
【補正対象書類名】明細書
【補正対象項目名】００３０
【補正方法】変更
【補正の内容】
【００３０】
本発明の第７態様によれば、車体拘束時には上記車体の傾斜角度及び傾斜角速度に対するフィードバックを停止させ、かつ、上記車体の走行のみを継続する制御に切り替えるとともに、上記車輪の回転角速度の誤差の積分値をリセットする第１又は２又は３の態様に記載の倒立二輪走行型ロボットを提供する。
【手続補正１３】
【補正対象書類名】明細書
【補正対象項目名】００３１
【補正方法】削除
【補正の内容】
【手続補正書】
【提出日】平成18年9月16日(2006.9.16)
【手続補正１】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項１】
車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記車体に有しかつ上記車体が地面に接触するか否かの接触有無を検出する少なくとも一つの接触センサと、
上記接触センサで検出された上記車体に対して接触していることが接触情報として入力されると、上記入力された接触情報により上記車体の傾斜方向への回転が拘束されている車体拘束状態を認識する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により、上記車体が移動停止して上記地面に接触している状態から、上記駆動装置を駆動して上記車体を倒立位置まで起き上がらせて上記車体を走行停止状態で倒立状態を維持させる倒立停止制御を行なう走行制御方法と、上記車体に対する接触を考慮することなく上記車体の上記倒立状態を維持しながら上記車体を走行させる第１倒立走行制御を行なう走行制御方法と、上記車体に対する接触を考慮して上記車体の上記倒立状態を維持しながら上記車体を走行させる第２倒立走行制御を行なう走行制御方法とを切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する倒立二輪走行型ロボット。
【請求項２】
車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記第１状態検出手段により検出された上記車体の傾斜角度が所定角度以上傾いたとき、上記車体の傾斜方向への回転が拘束されている車体拘束状態を認識する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により、上記車体が移動停止して上記車体拘束状態から、上記駆動装置を駆動して上記車体を倒立位置まで起き上がらせて上記車体を走行停止状態で倒立状態を維持させる倒立停止制御を行なう走行制御方法と、上記車体拘束状態を考慮することなく上記車体の上記倒立状態を維持しながら上記車体を走行させる第１倒立走行制御を行なう走行制御方法と、上記車体拘束状態を考慮して上記車体の上記倒立状態を維持しながら上記車体を走行させる第２倒立走行制御を行なう走行制御方法とを切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する一方、車体拘束時には、上記車輪を回転駆動させるトルクに、上記車体への拘束力による発生するトルク分を加算する制御方法に切り替える倒立二輪走行型ロボット。
【請求項３】
車体と、
上記車体に同軸に配置された２つの車輪と、
上記車輪を各々駆動する駆動装置と、
上記車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、
上記車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、
上記車体に有しかつ上記車体が地面に接触することにより上記車体が受ける接触力を検出する少なくとも一つの接触センサと、
上記接触センサで検出された上記車体に対する接触力が接触情報として入力されると、上記入力された接触情報により上記車体の傾斜方向への回転が拘束されている車体拘束状態を認識する車体拘束認識手段と、
上記駆動装置への指令値を決定し、決定した指令値により、上記車体が移動停止して上記地面に接触している状態から、上記駆動装置を駆動して上記車体を倒立位置まで起き上がらせて上記車体を走行停止状態で倒立状態を維持させる倒立停止制御を行なう走行制御方法と、上記車体に対する接触を考慮することなく上記車体の上記倒立状態を維持しながら上記車体を走行させる第１倒立走行制御を行なう走行制御方法と、上記車体に対する接触力を考慮して上記車体の上記倒立状態を維持しながら上記車体を走行させる第２倒立走行制御を行なう走行制御方法とを切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、
上記制御手段は、上記車体が移動停止して上記地面に接触している状態から、上記駆動装置を駆動して上記車体を倒立位置まで起き上がらせて上記倒立停止制御を行い、上記倒立状態が安定したのち、上記接触センサによる接触情報の有無の検出を上記車体拘束認識手段で開始し、上記接触センサによる接触情報が無く上記車体拘束認識手段で上記車体拘束状態を認識しないとき、上記倒立停止制御から上記第１倒立走行制御に切替えて走行状態を維持させるとともに、上記接触センサによる接触情報が有り上記車体拘束認識手段で上記車体拘束状態を認識したとき、上記倒立停止制御又は上記第１倒立走行制御から上記第２倒立走行制御に切替えて、上記接触力の方向と上記駆動装置へのトルク指令値（τ1）とを比較し、その回転方向が反対の場合には、上記駆動装置へのトルク指令値に上記接触力により発生するトルク（τａ）を加算した値を新たなトルク指令値として、その新たなトルク指令値により上記駆動装置が駆動される倒立二輪走行型ロボット。
【請求項４】
上記車体拘束認識手段で上記車体拘束状態を認識して上記制御手段により上記第２倒立走行制御を行なうとき、上記車体拘束状態を考慮して上記車体の上記倒立状態を維持しながら上記車体の回転速度に対するＰＩＤ制御の速度制御により上記車体を走行させるとともに、上記車体の回転速度に対する上記ＰＩＤ制御の速度制御において、上記車輪の回転角速度の誤差の積分値をリセットすることにより、上記車体の上記地面に対する接触する前に発生している上記車体の位置ずれである速度差の積分値を一度ゼロにして接触時からのＰＩＤ制御にする請求項１に記載の倒立二輪走行型ロボット。
【国際調査報告】
2007013282.tif 000294
2007013282.tif 000295