WO2007013282A1

WO2007013282A1 - 倒立二輪走行型ロボット及びその制御方法

Info

Publication number: WO2007013282A1
Application number: PCT/JP2006/313579
Authority: WO
Inventors: Yuji Adachi
Original assignee: Matsushita Electric Industrial Co., Ltd.
Priority date: 2005-07-26
Filing date: 2006-07-07
Publication date: 2007-02-01
Also published as: CN101052929A; US7635041B2; JPWO2007013282A1; CN100557539C; JP3993883B2; US20080173493A1

Abstract

　車体と、車体に同軸に配置された２つの車輪と、車輪を各々駆動する駆動装置と、車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第１状態検出手段と、車輪の回転角度及び回転角速度の少なくとも１つを検出する第２状態検出手段と、車体の回転が拘束されているかどうかを検出する車体拘束認識手段と、駆動装置への指令値を決定する制御手段を備え、車体拘束認識手段の検出結果に基づいて、制御手段が車体回転に寄与するトルクの割合を変化させた指令値にする。

Description

明細書

倒立二輪走行型ロボット及びその制御方法

技術分野

[0001] 本発明は、荷物運搬作業を行うロボットのうち、同軸二輪車を用いた倒立二輪走行型ロボット及びその制御方法に関するものであり、詳しくは、ロボット姿勢拘束時の倒立二輪走行型ロボット及びその制御方法に関するものである。

背景技術

[0002] 荷物運搬作業を行うロボットしては、三輪型や四輪型など 3点以上で必ず接触して安定を保つものが一般的である。し力しながら、三輪型、四輪型などは、面積を有すること、小回りが難しいことから倒立振子モデルに基づいて動作する同軸二輪車型のロボットが研究されてきて、る。

[0003] 同軸二輪車型の移動ロボットとしては、例えば特許文献 1、特許文献 2に記載のものが知られている。

[0004] 図 13は、上記特許文献 1に記載された従来の同軸二輪車型の移動ロボットを示すものである。

[0005] 特許文献 1に記載された同軸二輪車型移動ロボットは、両端に一対の車輪 102, 1 03を備えた車軸 101上の回動可能に支持された車体 104と、車体 104に装着された車輪駆動用モータ 105と、車輪駆動用モータ 105に作動指令を送る制御コンピュータ 106と、車体 104の傾きを検出する角度検出手段 107を備えている。そして、角度検出手段 107により検出される車体 104の傾斜角度を短時間間隔にてサンプリングし、車体 104の傾斜角度を状態変数入力値、フィードバックゲイン Kを係数として、制御コンピュータ 106内に予め入力設定された制御入力算出式に基づき、車輪駆動用モータ 105の制御トルクを算出する状態フィードバック制御である。この算出された制御トルク相当の作動を制御コンピュータ 106から、車輪駆動用モータ 105に指令することにより、倒立の姿勢制御が維持される。

[0006] 図 14は、上記特許文献 2に記載された人乗り型の同軸二輪車型移動ロボットを示すものである。 [0007] 特許文献 2に記載された人乗り型の同軸二輪車型移動ロボットは、プラットホーム 2 01上に同軸に配された左右の駆動輪 202を姿勢感知センサーの出力に応じて制御駆動することで前後方向のバランスの保持のための姿勢制御と走行制御とを行う。そして、駆動輪 202の前方側又は及び後方側で接地する補助輪 203と、補助輪 203を出没させる補助輪駆動部 204とを備えている。そして、障害物検知センサー 205と走行速度及び姿勢の制御の状態に応じて補助輪駆動部 204を駆動して補助輪 203を出没させる制御回路を備えているため、転倒を招く原因となりやすい障害物があった時、補助輪 203が出て安定性を高められるために、転倒の危険性を少なくすることができる。

[0008] 特許文献 1 :特許 2530652号

特許文献 2：特開 2004 - 74814

発明の開示

発明が解決しょうとする課題

[0009] 上述の特許文献 1に示された技術にぉ、ては、状態フィードバック制御を常に行つている。

[0010] そして、同軸二輪車の車体 104と車輪 102, 103が共に拘束無く相対的に回転できる場合には、車輪駆動用モータ 105で発生したトルクは、車輪 102, 103の回転に寄与すると同時に、反作用のトルクが車体 104に働いて、全体として倒立姿勢制御を維持できる。

[0011] し力しながら、車体 104の一部が壁や地面に接触し、それ以上回転できなくなるような拘束力を外部から受けると車体 104が回転しないだけでなぐ拘束力によるトルクが車輪 102, 103に伝達される。よって、拘束力がない場合に比べて、車輪 102, 10 3に対して過大なトルクがかかり、車輪回転が増し、走行速度が急上昇するという課題を有していた。

[0012] 上述の特許文献 2に示された技術においては、障害物検知センサー 205で障害物を検知した場合、補助輪駆動部 204を駆動して補助輪 203を出没させる機構を有しているため、補助輪 203が出没した場合には、左右の駆動輪 202と補助輪 203の 3 点接地になり、転倒の危険性を少なくできる。 [0013] しかし、本技術では障害物を認識して補助輪 203を出すだけで、速度や姿勢は人の操縦に依存している構成となっている。本構成においても、人がバランスを崩してプラットホーム 201を含めた本体部が地面に接触した場合や、プラットホーム 201を含めた本体部が壁等に接触して回転できなくなるような拘束力を受けた場合、特許文献 1の場合と同様に駆動輪 202に過大なトルクがかかり、走行速度が急上昇するという課題を有していた。この場合は、人の操縦により停止指令を送るなどの、危険を回避することになる力操作が人に依存するという点でユーザに負担をかけるという課題を有していた。

[0014] 本発明の目的は、上記従来の課題を解決するもので、倒立制御を行いながら移動する同軸二輪車の車体の一部が地面や壁などに接触して拘束力を受けた場合に、姿勢制御や走行状態を維持可能でかつユーザに負担をかけない安全な倒立二輪走行型ロボット及びその制御方法を提供する。課題を解決するための手段

[0015] 上記目的を達成するために、本発明は以下のように構成する。

[0016] 本発明の第 1態様によれば、車体と、

上記車体に同軸に配置された 2つの車輪と、

上記車輪を各々駆動する駆動装置と、

上記車体の傾斜角度及び傾斜角速度の少なくとも 1つを検出する第 1状態検出手段と、

上記車輪の回転角度及び回転角速度の少なくとも 1つを検出する第 2状態検出手段と、

上記第 1状態検出手段又は上記第 2状態検出手段で検出された情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを検出する車体拘束認識手段と、

上記駆動装置への指令値を決定し、決定した指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動され上記車体の走行制御が行われる制御手段とを備え、

上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボットを提供する。

[0017] 本発明の第 10態様によれば、車体の傾斜角度及び傾斜角速度の少なくとも 1つを第 1状態検出情報として検出し、

上記車輪の回転角度及び回転角速度の少なくとも 1つを第 2状態検出情報として検出し、

上記第 1状態検出情報又は上記第 2状態検出情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを車体拘束認識情報として検出し、上記検出された車体拘束認識情報に基づ!、て、制御手段が複数の走行制御方法を切替えて、上記車体に同軸に配置された 2つの車輪を各々駆動する駆動装置への指令値を決定し、決定された上記指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボットの制御方法を提供する。

発明の効果

[0018] 本発明の構成によれば、倒立して姿勢制御を行いながら走行する倒立二輪走行型ロボットの車体の一部力地面や壁などに接触して回転を拘束された場合に、車体拘束認識手段又は車体拘束認識動作によって車体の回転方向の拘束が認識される。そして、認識された拘束状態に応じて、例えば車体回転に寄与するトルクの割合を変化させることで過大なトルクが車輪に力かって姿勢制御を維持できず走行速度が急上昇するという課題を回避することができる。

[0019] よって、本発明の構成によれば、倒立制御を行いながら移動する同軸二輪車型の倒立二輪走行型ロボットにおいて、車体の一部が地面や壁などに接触して回転を拘束された場合でも、姿勢制御や走行状態を維持でき、ユーザに負担をかけずに、安全に移動することができる。

図面の簡単な説明

[0020] 本発明のこれらと他の目的と特徴は、添付された図面についての好ましい実施形態に関連した次の記述から明らかになる。この図面においては、圆 1A]図 1Aは、本発明の第 1実施形態における倒立二輪走行型ロボットの傾いた状態での側面図であり、

圆 1B]図 1Bは、本発明の第 1実施形態における倒立二輪走行型ロボットの制御部などのブロック図であり、

[図 2]図 2は、本発明の第 1実施形態における倒立二輪走行型ロボットの傾いた状態での正面図であり、

[図 3]図 3は、本発明の第 1実施形態における倒立二輪走行型ロボットの移動構成の説明図であり、

[図 4]図 4は、本発明の第 1実施形態における倒立二輪走行型ロボットの機構パラメータ図であり、

[図 5]図 5は、本発明の第 1実施形態における倒立二輪走行型ロボットの制御ブロック図であり、

[図 6]図 6は、本発明の第 1実施形態における倒立二輪走行型ロボットの旋回制御模式図であり、

[図 7]図 7は、本発明の第 1実施形態における走行制御全体図であり、

[図 8]図 8は、本発明の第 2実施形態における倒立二輪走行型ロボットのフローチヤートであり、

[図 9]図 9は、本発明の第 1実施形態における倒立二輪走行型ロボットの接触状態図であり、

[図 10]図 10は、本発明の第 2実施形態における倒立二輪走行型ロボットのフローチヤートであり、

[図 11]図 11は、本発明の第 3実施形態における倒立二輪走行型ロボットの車体が傾いて地面に必ず接触する場合の説明図であり、

[図 12]図 12は、本発明の第 1実施形態における倒立二輪走行型ロボットのフローチヤートであり、

[図 13]図 13は、従来の特許文献 1に記載の同軸二輪車型の移動ロボットの図であり

[図 14]図 14は、従来の特許文献 2に記載の人乗り型の同軸二輪車型移動ロボットの図である。

発明を実施するための最良の形態

[0021] 本発明の記述を続ける前に、添付図面において同じ部品については同じ参照符号を付している。

[0022] 以下に、本発明にかかる実施の形態を説明する前に、本発明の種々の態様について説明する。

[0023] 本発明の第 1態様によれば、車体と、

上記車体に同軸に配置された 2つの車輪と、

上記車輪を各々駆動する駆動装置と、

[0024] 本発明の第 2態様によれば、上記車体に対する接触力を検出する少なくとも一つの接触センサを上記車体に有するとともに、上記車体拘束認識手段は、上記接触センサで検出された接触情報を入力として、車体拘束状態を認識する第 1の態様に記載の倒立二輪走行型ロボットを提供する。

[0025] 本発明の第 3態様によれば、車体拘束時には、上記車輪を回転駆動させるトルクに、上記車体への拘束力による発生するトルク分を加算する制御方法に切り替える第 1 又は 2の態様に記載の倒立二輪走行型ロボットを提供する。

[0026] 本発明の第 4態様によれば、車体拘束時には上記車体の傾斜角度及び傾斜角速度に対するフィードバックを停止させる制御に切り替える第 1又は 2の態様に記載の倒立二輪走行型ロボットを提供する。

[0027] 本発明の第 5態様によれば、車体拘束時には上記車体の傾斜角度及び傾斜角速度に対するフィードバックを停止させ、かつ、上記車体の走行のみを継続する制御に切り替える第 4の態様に記載の倒立二輪走行型ロボットを提供する。

[0028] 本発明の第 6態様によれば、車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする第 1又は 2の態様に記載の倒立二輪走行型ロボットを提供する。

[0029] 本発明の第 7態様によれば、車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする第 3の態様に記載の倒立二輪走行型ロボットを提供する。

[0030] 本発明の第 8態様によれば、車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする第 4の態様に記載の倒立二輪走行型ロボットを提供する。

[0031] 本発明の第 9態様によれば、車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする第 5の態様に記載の倒立二輪走行型ロボットを提供する。

[0032] 本発明の第 10態様によれば、車体の傾斜角度及び傾斜角速度の少なくとも 1つを第 1状態検出情報として検出し、

[0033] 以下に、本発明にかかる実施の形態を図面に基づいて詳細に説明する。 [0034] (第 1実施形態）

以下、本発明の第 1実施形態における倒立二輪走行型ロボット及びその制御方法について、図面を参照しながら説明する。

[0035] 本発明の第 1実施形態における倒立二輪走行型ロボットの傾いた状態での側面図を図 1A、傾いた状態での正面図を図 2に示す。ロボットの車体 1に対し、概して同一軸上に 2つの車輪 2a, 2bが配置される。ここで、車体 1の図中矢印 Aで示した進行方向に対して左車輪 2a、右車輪 2bとする。なお、車体 1の進行方向から見た正面図が図 2である。車輪 2a, 2bと車体 1との間に独立してそれぞれ駆動力を発生させる駆動装置の一例として機能するァクチユエータ 3a, 3bは車体 1の車輪 2a, 2bの近傍に取付けられ、各車輪 2a, 2bに連結されている。なお、説明のためァクチユエータ 3a, 3b は一例としてモータであるとする。なお、モータ 3a, 3bには、減速機 4a, 4bが取り付けられて、上記駆動装置の一例を構成するようにしてもよい。また、モータ 3a, 3bには、上記車輪 2a, 2bの回転角度及び回転角速度の少なくとも 1つを第 2状態検出情報として検出する第 2状態検出手段の一例として機能するエンコーダ 5a, 5bが取り付けられており、車体 1と車輪 2a, 2bの回転角度を計測できる。回転角度を一定サンプリング時間（例えば、 1msecおき）に計測し、その差分をサンプリング時間で除算することで、回転角速度が求まり、これを第 2状態検出情報として扱うことができる。

[0036] また、車体 1の車軸方向を法線とする鉛直平面内における姿勢を検出する姿勢センサ 6の一例である 2つのセンサ、すなわち、車体 1の傾斜角度及び傾斜角速度の少なくとも 1つを第 1状態検出情報として検出する第 1状態検出手段の一例として機能する 1軸ジャイロ 6aと 2軸加速度センサ 6bが車体 1の上部に取り付けられている。 1軸ジャイロ 6aは、車軸方向を法線とする鉛直平面に、車軸方向の傾斜角速度を第 1状態検出情報として検出できるように車体 1に取り付けられ、車体 1の車軸方向を法線とする鉛直平面内における傾斜角速度（円弧 6alで図示）を検出することができる。また、 2軸加速度センサ 6bは、車軸方向を法線とする鉛直平面の所定の 2方向（6bl, 6b2)に対する加速度を検出できるように取り付けられ、重力方向（6b3)を検出することを可能としている。第 1状態検出情報としての車体 1の姿勢角度 (傾斜角度)は、 1 軸ジャイロ 6aの積分値と 2軸加速度センサ 6bからの値をそれぞれ、制御コンピュータ部 9でハイパスフィルター、ローパスフィルターにかけ、それらを制御コンピュータ部 9 で合成して求める。

[0037] また、車体 1の前後の下部には接触センサ 10a, 10bが設けられ、車体 1の当該車体 1の前後の下部に対する他の物体などからの接触力を、接触情報としてそれぞれ柳』定することができる。

[0038] 車体 1の内部には、モータ 3a, 3bを駆動するためのモータドライバ 7a, 7b及びバッテリ 8、制御手段及び車体拘束認識手段の一例して機能する制御コンピュータ部 9が搭載されている。図 1Bに示すように、制御コンピュータ部 9は、 AZD変と DZ A変翻 9bとエンコーダカウンタ部 9cと演算部 9dと力も構成され、エンコーダ 5a, 5b の値はエンコーダカウンタ部 9cを通じて演算部 9dに入力される。また、 1軸ジャイロ 6 aと 2軸加速度センサ 6b、接触センサ 10a, 10bの値は AZD変翻 9aを通じて演算部 9dに入力される。演算部 9dでは、入力された接触センサ 10a, 10bからの値に基づき、上記車体 1の傾斜方向への回転が拘束されているかどうかを検出して、車体拘束認識情報を作成することができる。この作成された車体拘束認識情報は、例えば、接触センサ 10a, 10bの入力により判断される車体 1に対する拘束力の情報 (第 1実施形態参照）、接触センサ 10a, 10bの入力により判断される接触の有無の情報 (第 2実施形態参照）、車体 1の傾斜角度情報 (第 3実施形態参照)などであって、後述するように、制御方法を切替えるときに使用することができる。制御コンピュータ部 9の演算部 9dは、演算部 9dに入力された値より、姿勢制御及び走行制御の必要トルクを計算し、 DZA変より、トルク指令値をモータドライバ 7a, 7bに指令する。モータドライバ 7a, 7bは、指令値に基づいてモータ 3a, 3bを駆動する。なお、入力値から出力値への計算方法は、後述する。

[0039] 次に、本構成での倒立二輪走行型ロボットの制御方法について図 3で説明する。

図 3はロボットを鉛直上方から見た図である。本機構においては、車軸方向を法線とする鉛直平面 (B— B '面）内での姿勢及びその鉛直平面内の水平方向速度に関する並進制御 11 (姿勢制御 +前後移動制御）と旋回方向の動作に関する旋回制御 12 の 2つの制御に分けて考える。前者は、並進方向の

[数 1] 速度 V

に追従するように、姿勢制御と走行制御を行って走行状態を維持するものである。後者は、旋回方向の

[数 2]

角速度

に追従するように制御するものである。後述するように、この 2つの制御を合わせることで、移動制御（走行制御）を行って走行状態を維持することができる。

[0040] 次に、この移動制御について、順に、説明を行う。

[0041] まず、並進制御 11について、機構パラメータを示す図 4を用いて説明する。なお、車軸方向を法線とする鉛直平面内での動作であるので、 2次元モデルとして扱う。図 4のように、パラメータを以下のように設定する。車輪 2a, 2b、モータ 3a, 3b、減速機 4a, 4b、エンコーダ 5a, 5b、はそれぞれ 1つとみなして、車輪 2、モータ 3、減速機 4、エンコーダ 5とする。また、車体 1の重心を車体重心 13、車輪 2の車軸を車軸 14、車輪 2の重心を車輪重心 15とする。

[0042] M ：ロボットの車体 1の質量 [kg]

b

M ：車輪 2の質量（2つの車輪の合計） [kg]、

w

I：ロボットの車体 1の車体重心 13周りの慣性モーメント [kgm²]

b

I ：車輪 2の車輪重心 15周りの慣性モーメント（2つの車輪の合計） [kgm²] w

I ：モータ 3の慣性モーメント [kgm²]

r:車輪 2の半径 [m]

L：車軸 14からロボットの車体重心 13までの距離 [m]

g

[0043] [数 3]

：モータ 3と減速機 4と車軸 2との間の粘性摩擦係数 [NmZ (radZs) ]

画

：車輪 2と地面 800との間の粘性摩擦係数 [NmZ (rad/s) ] [数 5] て t

：モータ 3のトルク定数 [NmZ A]

[数 6]

：減速機 4のギア部減速比

g :重力加速度

[数 7]

Φ

：ロボットの車体 1の傾き [rad]

[数 8]

Θ

：車輪 2の地面 800に対する回転角 [rad]

u：モータ 3に入力する電流 [A]

また、記号を次のように定義する。

T:運動エネルギー、 U:位置エネルギー， D:摩擦損失

z：車体重心高さ， z：車輪重心高さ， s：車体重心並進位置，

1 2 1

S：車軸並進位置，

2

ここで、倒立 2輪走行型ロボットの運動方程式を、ラグランジェ法を用いて導出する。ラグランジの運動方程式は

[数 9]

(式 1 )

[数 10] ά ( ΤΛ

(式 2 ) 各値とその微分値は次のように表される。

[数 11]

= r0 + L_g sin φ (式 3 ) [数 12]

s₂ = τθ (式 4)

[数 13]

(式 5 )

[数 14]

(式 6 )

[数 15]

∑ = r + L_g cos^ (式 7 ) [数 16]

(式 8 )

[数 17]

ζ_χ =

φ (式 9 )

[数 18]

ζ₂ =0 (式 1 0) 各エネルギーは

[数 19]

Γ (式 1 1 )

[数 20]

U - M_wgr +M_bgir + 1οο&φ) (式 1 2)

[数 21] 簡単なモデルとして、車体 1にかかる外力が 0とした場合、

[数 22]

Q , = - η τ _t u (式 1 4 )

[数 23]

Q 2 = η τ _t u (式 1 5) よって、運動エネルギー τは

[数 24]

Τ + L^{2 2} )+丄 +丄 i ² +丄 Ι„η² (θ― φ)

(式 1 6 ) となる。

[0045] ロボットの車体 1の傾き φについて、ラグランジェ法により運動方程式を導出する。

[数 25]

M_b (r0L_g cos +

I ― I f (θ― if)-M_bgl sin φ-(μ — φ))

= (M_bL² _g +I_h +I_m^²) + (M_brL_g cos^ - 1 j¹ )0-M_bgl sin φ - μβ + μ₃φ = -ητ,η

(式 1 7 )

[0046] 車輪 2の地面 800に対する回転角 Θについて、ラグランジェ法により運動方程式を導出する。

[数 26]

M_w

sin φ)+ΐ + Ι_Μη²{θ - ) + (μ (θ - φ)+ηβ)

= (M_brL_g cos - I )φ + {{M_b +M r² +I_W+ I_m"² )<9 - MrLJ>² sin φ + μ

- μ^> + νιβ = ηて _tu

(式 1 8 )

(式 17)と (式 18)を足し合わせると、 [数 27]

(M_brL_g ^ +Μ,ΐ] +I_b)^ + ((M_b +M r² +I_w +M_brL_g cos

-M_bgl sin φ-M L φ² sin φ + u Θ - 0

(式 1 9 ) となる。

[0047] よって、 2つの式（式 17)と（式 19)を

[数 28] = 0 , = 0 の周りで近似して

[数 29] ί sin φ - , cos φ = \ , ~ = 0 ) [数 30]

- μβ + = -ητ,υ (式 2 0 ) (M_brL_g +M_bL² _g+I_b)^ + ((M_b +M_w)r² +M_brL_g + 1 _w)0 + = 0 (式 2 1 ) となる。

[0048] ここで変数を

[数 31]

(式 22 ) + Ι

Δ α₁α₁₂ -α₁₂α_2λ (式 23 ) ( W_bgL_g (α_η - ₂₁ )M_hgL_g

(式 24)

μ +

[数 32] て _tヽ (式 2 5 )

A とおいて、

[数 33]

= Ax + B M (式 2 6 )

[数 34]

1 0、 ⁰〕

A = α₃ B = X = Φ (式 2 7 )

、"2 ₄ 、^b

の状態方程式の形に帰着できる。なお、

[数 35]

(Φ、

X = は、状態ベクトルである。

[0049] 以上のモデル化は、状態ベクトルの各値が 0での近似モデルであるので、前後方向の位置 (速度指令）に関する追従性を考慮するために、このモデルに対してステツプ状の目標入力に追従するモデルの制御系を構成する。なお、目標入力を一定速度目標値であるとする。

[数 36] 速度 y ( = Θ ) は、可観測であるとし、

[数 37] y = Θ = Cx = (0 , 0 ,1 )x と表せる。

[0050] [数 38] 速度 y (りをステップ状の

[数 39] 目標角速度 =v/r に定常偏差なく追従させるために、積分器を用いた制御系とする。なお、そのときの制御ブロックを図 5に示す。ここで、

[数 40]

K(lx3)、 G(lxl) の閉ループシステムを安定にするためのフィードバックベクトルである。

この構成において、

[数 41]

z = ί(θ _d - Q)dt より

[数 42]

/ = ™ x + 0 ( ζ ώ S ) 制御入力は、

[数 43]

u = -Kx + Gz (式 2 9 ) と表せる。ここで、 Inは 3行 3列の単位行列、 Im=lとして

[数 44]

ύ = K + G± = -K(Ax +Bu) + G(-Cx +θ

(式 3 0 )

(式 26)と（式 30)より

[数 45] X 0 A B X 0

Θ (式 31 ) ΰ - κ 一 G C 0 u G

[数 46] y

(式 32) ここで、定常値は、

[数 47]

(式 3 3)

[数 48] y(∞) = [c o][x(∞)^r u(∞)^T] (式 34) なお、

[数 49] y(oo) = [C θ][χ(αο)^Γ u(∞)^r f =[C 0] (式 35)

C

となり、定常偏差がないことがわかる。

次に、定常値からの誤差システムを考える _c

[数 50] = X - x(oo), u_e =u - u(oo), e = θ - 9_d として式変形を行うと

[数 51]

0

K G

(式 3 6 ) i„ 0 A B

-K -G C 0

となる。

[数 52]

A B

H = [K (式 3 7)

C 0

[数 53] w = -H| (式 3 8 ) として、

[数 54]

(式 3 9

となり、出力を eとして

[数 55]

(式 40) となる。（式 39)、（式 40)に wの状態フィードバックをしたレギユレータとみなせる。よつて、この誤差システムのフィードバックゲイン Hを、最適レギユレ一タ法ゃ極配置法、その他の種々の設計法によって導出する。このゲインを用いれば、

[数 56] 偏差）の過渡応答誤差の少ない最適なサーボシステムが設計できる。なお、（式 37)より [数 57]

[K G] = H (式 4 1 )

となる。したがって、

[数 58] ゲイン [い ₂, ₃,ん ₄】を用いて、モータ 3へ出力される

[数 59] トルク指令値 _Γι を、

[数 60] r, = ητ,η = -ηΐι [A, ,k₁,k-₁,k₄})i₁

2 ) とすれば、倒立の姿勢制御と速度制御が同時に実現できる。

ただし、

[数 61]

(式 4 3)

[数 62]

(式 4 4 )

である。なお、

[数 63] k_} < 0, k₂ < 0, 3 < 0， k₄ < 0 である。本構成では、車輪 2は 2つであるので、

[数 64] トルク指令値て ,

の 0. 5倍を各車輪 2 (2a, 2b)に出力すれば、並進方向の姿勢制御と速度制御を行える。そのときの、各車輪 2a, 2bの

[数 65] トルク指令値を左車輪で_1£右車輪として、

[数 66]

て) = 0.5て】（式 4 5 ) て _1R = 0.5て！（式 4 6 ) となる。

[0055] 次に、旋回制御 12について旋回制御模式図である図 6を用いて説明する。

[0056] ロボットの車体 1を上面から見て、ロボットの車体 1の車輪 2間の長さを

[数 67]

とし、水平面上でのロボットの車体 1の移動速度を

[数 68]

V

とし、左車輪 2aの回転速度を

[数 69] とし、右車輪 2bの回転速度を

[数 70]

とし、旋回半径を R、旋回速度を

[数 71]

ω

とする。また、左車輪 2aの回転角速度を

[数 72]

、右車輪 2bの回転角速度を

[数 73]

とすると、

[数 74]

(式 4 7 )

となる。

[数 75]

旋回速度目標値を iW _d

として、

[数 76]

実際の旋回速度

との差に対してフィードバック (PD制御等）を行、、

[数 77] 旋回方向のトルク Γ₂ を決定する。

[0058] [数 78] 旋回方向のトルクて ₂ は、左右の車輪 2a, 2bに発生するトルク差であるとみなせる。左右の車輪 2a, 2bに、それぞれ、その旋回方向のトルクの 0. 5倍を、旋回方向の目標トルクを算出できる両車輪 2a, 2bに、それぞれ加算、減算してやることで、旋回方向の制御が実現できる。

[0059] 並進制御 11、旋回制御 12の統合された制御ブロック図を図 7に示す。

[0060] そして、左輪 2a、右輪 2bに指令するトルク指令値を

[数 79] ていて R

とすると

[数 80] 、 2

1

[0061] 以上のように、トルクを決定することにより、上記ロボットは、倒立姿勢を維持しながら旋回及び直線走行をすることができる。

[0062] 図 1 Αのように、車体 1が地面 800から離れている場合には、これまで述べたような制御則に基づけば、倒立姿勢を維持しながらの走行を行うことができる。

[0063] 次に、第 1実施形態では、図 8のように車体 1の一部が回転方向に拘束を受けた場合の一例として、車体 1が傾いて車体下部が地面 800に接触した場合を示す。仮定として、車体 1と地面 800との摩擦力は 0で

[数 81] 垂直抗力 ^

だけが、車軸 14カゝら水平移動した [数 82]

距離の接触点において拘束力として発生するとする。接触センサ 10b (接触センサ 10aも同様であるため、代表的に接触センサ 10bで説明する。）は、車体 1に対する他の物体や地面 800や壁など力もの接触力を計測できるセンサとし、接触力センサ 10bによつて

[数 83]

垂直方向の拘束力が検出され、幾何学的な条件より

[数 84] 距離も既知である。このとき、車体 1には拘束力による

[数 85]

卜ノレクて. = F r が発生する。また、位置ずれ量が

[数 86]

5₃ > 0 の状態のときに、図 8のような

[数 87]

(傾斜角を < 0 )

接触が発生した場合を考える。このとき、（式 42)に基づくモータ 3のトルク指令値は、車輪 2を正転させる方向（ロボットの車体 1が図 8において右方向に移動し、車体 1が車軸 14周りに左回転する方向）を正として、

[数 88]

τ_ι = ητ, [- Λ[ -k₂ ,-k, ,-k₄ Jxj = -ητ_! {k^ + Η₂φ + k^0 + k₄z_l ) (式 4 9 j となる,

[0064] [数 89] 垂直効力 > 0 であるので、傾斜方向の回転が拘束され、

[数 90] φ = Φ_α、二 0

となる。

このとき

[数 91]

τ_χ =- _t{k^_a +k_i0 + k_Az^) (式 50) となる。

[0065] [数 92] 車体 1 にかかるトルクて ₆

は、モータ 3からのトルクの

[数 93] 反作用力（_ ）と、

[数 94] 接触によるトルク r_fl の和になるため、

[数 95]

T_b 二て。 - r,

+ k₃0 + k₄z ) (式 5 1 ) となる。この

[数 96] r_b は、、の値によっては、

[数 97] r„ > 、 T_b < 0

のいずれにもなりうる。

[0066] (式 51)において

[数 98]

である場合には、車体 1には、右回転させるようにするトルクが働くため、車軸 14周りに右回転し、接触は解消され倒立状態に戻る事ができる。しかしながら、

[数 99] b < である場合には、車体 1には、左回転のトルクがかかり、車輪 2には、反力として右へ移動する回転トルクが発生する。この時、車体 1が

[数 100] 外力で拘束されているので、車輪 2には、

[数 101] 外力 F によって発生した

[数 102] 卜ノレク。 = f\r、も、車輪 2への回転トルクとして伝達される。そのため、車輪 2に余分のトルクがかかり、車輪 2の過度の回転現象を生じ、暴走現象に陥ってしまう。

[0067] そこで、本発明の上記第 1実施形態に力かるロボットの制御方法では、接触力センサ 10bより接触情報を検知し、検知した接触力方向と [数 103]

トルク τ_λ とを比較し、その回転方向が反対の場合には、モータ 3への

[数 104] トルク指令値 η を、接触力により発生する

[数 105] トルク分て _a = F_xr を加算した

[数 106]

τ = -ητ_{、k + + k₄z, ) + （式 5 2 ) とする。

[0068] したがって、本発明による上記第 1実施形態に力かるロボットの制御方法によれば、

(式 52)に基づきモータ 3のトルクを算出することにより、地面 800からの接触力による影響を除去でき、ユーザに負担をかけずに、安全な倒立二輪走行型ロボットを実現できる。

[0069] また、本構成において車体 1と地面 800との間に

[数 107] 摩擦力 F₂ が発生する場合、図 9のように、その

[数 108]

合力 F₃ によって発生する回転の反対方向のトルクを補償して走行することとすれば同様の効果を得られる

[数 109] r, - -ητ, (k^_a + k 0 + k₄z, ) + F₃r₃ (式 5 3 ) また、外乱オブザーバを用いて、接触センサ無しに外乱力を推定してその推定された接触力を用いて (式 52)、（式 53)と同様の制御を行ってもよい。

[0070] ここで、上記した第 1実施形態での切替制御を用いた走行動作フローチャートを図 12で説明する。

[0071] 初期状態 (ステップ S1)では、ロボットの車体 1は傾斜して停止している。初期設定として、

[数 110]

(z, V ω) = (0,0,0)

(=ステップ S2)とする。次に、（式 50)で示す倒立停止

[数 111]

{ θ_ά = v / r = 0 ) 制御 (ステップ S3)を行って起き上がる。倒立停止制御 (ステップ S3)の安定時間（例えば 5秒程度)後 (ステップ S4)、車体 1の地面 800などに対する接触の有無を判別する（ステップ S 5)。

[0072] ステップ S5で接触が無い場合には、

[数 112]

速度 (V ω )

設定を行った (ステップ S8)後、（式 49)で示す倒立走行制御 (ステップ S9)を行って走行状態を維持した後、ステップ S5に戻る。

[0073] ステップ S5で接触がある場合には、

[数 113] 速度 (V ω) 設定を行った (ステップ S6)後、（式 52又は式 53)で示す倒立走行制御 (ステップ S7 )を行って走行状態を維持した後、ステップ S5に戻る。

[0074] なお、ステップ S6とステップ S8において、接地しているときも、接地していないときも、その差分トルクを補正するので、どちらの場合も、同様の走行が行える。

[0075] 以上のように、上記第 1実施形態によれば、制御コンピュータ部 9の制御の下で、以下のような動作制御を行うことができる。

[0076] すなわち、初期の車体 1が移動停止して地面 800に接地している状態から、まず、倒立停止制御（式 (49)、ステップ S2〜S3)を行うことにより、車体 1を倒立位置まで起き上がらせて、倒立状態 (ただし、走行は停止している状態。）を維持する。次いで、所定時間 (安定時間)経過して倒立状態が安定した (ステップ S4)のち、接触センサ 10a, 10bによる接触の有無の検出を開始する（ステップ S5)。ステップ S5で接触（言 V、換えれば、車体拘束)が無ければ、倒立停止制御力も倒立走行制御 (式 (49)、ステツプ S8〜S9)に移って走行状態を維持したのち、接触センサ 10a, 10bによる接触の有無の検出を再度行う（ステップ S5)。ステップ S5で接触 (言い換えれば、車体拘束)が検出された場合には、倒立停止制御力倒立走行制御 (式 (52)又は式 (53 )、ステップ S6〜S7)に移って走行状態を維持したのち、接触センサ 10a, 10bによる接触の有無の検出を再度行う (ステップ S5)。

[0077] このように、倒立停止制御 (ステップ S3)、倒立走行制御 (ステップ S9)、倒立走行制御 (ステップ S7)とを適宜切替ながら、倒立二輪走行することによって、車体 1に接触が発生しても、接触状態 (すなわち車体拘束状態）に応じて、車体回転に寄与するトルクの割合を変化させることで、過大なトルクが車輪 2にかかって姿勢制御を維持できず走行速度が急上昇すると、つた暴走を防止することができ、ユーザに負担をかけずに、安全に走行を継続することができる。

[0078] (第 2実施形態）

本発明の第 2実施形態を第 1実施形態と同じ、図 8で説明する。第 1実施形態においては、接触センサを設けて接触カを検知していたが、第 2実施形態では、接触センサ 10bが、接触の有無を検知のみで力測定ができないセンサの場合や、力計測できるセンサであっても接触の有無のみを利用する場合とし、その制御方法を述べる。

[0079] 車体 1の一部が地面 800などに接触している場合、車体 1は回転できず、車体 1は 2つの車輪 2a, 2bと、少なくとも 1点との 3点以上で地面 800に接していることとなる。従来力もあるような独立二輪駆動で、補助輪等で接地している台車の移動制御においては、車輪 2の回転速度に対して、 PID制御等によって走行制御がなされて走行状態を維持している。そのため、この第 2実施形態においても、接地時には同様の構成とみなすことができるので、接地した場合には、

[数 114] η ^ k₅ (e - 9_d ) + k₆j(0 - 0_d )dt = k₅ -- e_d ) + k₆z_l (式 5 4 ) に示すような、回転速度に対する略 PI制御の速度制御の構成とする。したがって、接触がある場合には、（式 54)、接触がない場合には (式 50)の制御を切り替えて制御を行う。

[0080] 第 2実施形態での切替制御を用いた走行動作フローチャートを図 10で説明する。

[0081] 初期状態 (ステップ S 11)では、ロボットの車体 1は傾斜して停止している。初期設定として、

[数 115] fz, V ίϋ) = (0,0,0)

(=ステップ SI 2)とする。次に、（式 50)で示す倒立停止

[数 116]

制御 (ステップ S 13)を行って起き上がる。倒立停止制御 (ステップ S 13)の安定時間（例えば 5秒程度)後 (ステップ S14)、接触の有無を判別する (ステップ S15)。もし接触 (ステップ S 15)のな、場合は、

[数 117] 速度 (V ω) 設定を行った (ステップ S 20)後、（式 49)で示す倒立走行制御 (ステップ S21)を行つて走行状態を維持している。（式 49)に基づく倒立走行制御 (ステップ S21)から、接触時の（式 54)に基づく速度制御 (ステップ S 18)へと制御方法が移る条件として、積分項リセット履歴ビット btlを設ける。積分項とは、速度差の

[数 118] 積分

(車体 1の現在位置に対して車体 1のバランスが崩れたりなどして位置ずれが生じるときの位置ずれ量)である。そして、倒立走行制御 (ステップ S21)中に、接触の有無を判断する (ステップ S22)。

[0082] ステップ S22で車体 1が地面 800などに接触していない場合には、積分項リセット履歴ビット btlをリセット (btl =0)した (ステップ S26)後（すなわち、積分項のリセットは行わずに）、ステップ S 20に戻る。

[0083] ステップ S22で車体 1が地面 800などに接触している場合には、積分項リセット履歴が ONか否カゝ、すなわち、積分項リセット履歴ビット btl = 1か否かを判断する（ステツプ S23)。積分項リセット履歴ビット btl = 0のとき (接触後に積分項のリセットを行っていなかったとき）のみ、

[数 119] 積分項を

[数 120]

リセッ卜 = 0

した (ステップ S27)後（積分項をリセットし、かつ、積分項をリセットしたことを示す履歴情報を残した後）、ステップ S20に戻る。積分項をリセットするのは、接触する前に発生して!/、る位置ずれ (速度差の

[数 121]

積分

)を、一度、 0にして、接触時からの PI制御にするためである。このように、積分項をリセットすること〖こよって、接触前の位置ずれ量を削除できて、接触後の車体 1の位置ずれ量のみにより、動作制御が行われるため、車体 1の暴走現象を防止することができる。

ステップ S23で積分項リセット履歴ビット btl = 1であるとき、接触の有無を判別する (ステップ S 24)。

[0084] ステップ S24で車体 1が地面 800などに接触してヽな、場合は、積分項リセット履歴ビット btlをリセット (btl =0)した (ステップ S28)後（すなわち、積分項のリセットは行わずに）、ステップ S20に戻る。

[0085] ステップ S24で、そのまま、車体 1が地面 800などに接触が続いている場合 (btl =

1)は、安定時間（例えば 1秒）（ステップ S25)以内であれば、ステップ S20に戻り、倒立走行 (ステップ S21)を継続する。

[0086] 一方、安定時間（例えば 1秒）が経過して、れば、倒立走行 (ステップ S21)力も速度制御 (ステップ S30〜S32)に移る。

[0087] ステップ S15で車体 1が地面 800などに接触している場合又はステップ S25で安定時間経過時には、

[数 122] 速度 (V ω) 設定を行った (ステップ S30)後、

[数 123]

(V ω \ = ("0_?0) か否かを判断する (ステップ S31)。 YESの場合には、起き上がれないと判断して終了する（ステップ S19)。ステップ S31で、 NO、すなわち、

[数 124]

(V ω)≠(0,0) の場合には、（式 54)で示す速度制御 (ステップ S32)で走行したのち、ステップ S14 に戻り、速度制御 (ステップ S32)の安定時間（例えば 5秒程度）が経過した力否かを判断する (ステップ S 14)。

[0088] 以上のように、上記第 2実施形態によれば、制御コンピュータ部 9の制御の下で、以下のような動作制御を行うことができる。

[0089] すなわち、初期の車体 1が移動停止して地面 800に接地している状態から、まず、倒立停止制御（式 (49)、ステップ S12〜S13)を行うことにより、車体 1を倒立位置まで起き上がらせて、倒立状態 (ただし、走行は停止している状態。）を維持する。次いで、所定時間 (安定時間)経過して倒立状態が安定した (ステップ S14)のち、接触センサ 10a, 10bによる接触の有無の検出を開始する（ステップ S15)。ステップ S15で接触 (言い換えれば、車体拘束)が無ければ、倒立停止制御力倒立走行制御 (式（ 49)、ステップ S20〜S21)に移って走行状態を維持する。ステップ S15で接触（言い換えれば、車体拘束）が検出された場合には、倒立停止制御力も速度制御 (ステップ S30〜S32)に移る。次いで、所定時間（安定時間）経過して安定して速度制御が行われた (ステップ S14)のち、接触センサ 10a, 10bによる接触の有無の検出を再度行い (ステップ S15)、ステップ S15で接触がなければ (起き上がりが成功したならば）、倒立走行制御（式 (49)、ステップ S20〜S21)に移って走行状態を維持する。ステップ S15で接触があれば (起き上がりが不成功ならば）、そのまま、速度制御 (ステツプ S30〜S32)を継続する。速度制御 (ステップ S30〜S32)を行うとき、速度及び角速度が 0の場合には、車体 1が物体又は地面 800などに接触している場合など、車体 1が起き上がれない場合であると判断して、車体 1の起き上がり駆動を停止し、接地状態とする。

[0090] また、倒立走行制御（式 (49)、ステップ S20〜S21)中に接触があった場合 (ステツプ S22)には、接触時までの位置ずれ量をリセットして 0にしたのち、接触時力もの位置ずれ量のみにより、動作制御が行われるため、車体 1の暴走現象を防止することができる。さらに、倒立走行制御時に、接触時までの位置ずれ量をリセットして 0にしたのち、接触が安定時間経過しても続いている場合には、車体 1の倒立状態が崩れていると判断して、倒立走行制御力速度制御に切り替えて、倒立状態に復帰させるように動作制御することができる。

[0091] このように、倒立停止制御 (ステップ S13)、倒立走行制御 (ステップ S21)、速度制御 (ステップ S32)とを適宜切替ながら、倒立二輪走行することによって、車体 1に接触が発生しても、接触状態 (すなわち車体拘束状態）に応じて、車体回転に寄与するトルクの割合を変化させることで、過大なトルクが車輪 2にかかって姿勢制御を維持できず走行速度が急上昇すると、つた暴走を防止することができ、ユーザに負担をかけずに、安全に走行を継続することができる。 [0092] (第 3実施形態）

第 2実施形態においては、実際の接触センサ 10a, 10bの入力によって接触の有無を検知して制御方法を切り替えたが、本発明の第 3実施形態では、走行する地面 800力平坦な面であるということがわ力つている場合として、接触センサ無しで、車体 1の傾斜角度により制御方法の切替を行うものであって、車体 1の傾斜角度が所定角度以上傾いたときには切替制御を行うものである。すなわち、図 11のように、車体 1が傾、て地面 800に必ず接触する場合の車体 1の傾斜角が、

[数 125]

φ>φ_Ι} > 0 φ<φ <ο

として、車体 1が傾いて地面 800に接触しないとき、すなわち、

[数 126]

( _ύ≥φ≥ _ΰ) のときには、（式 49)で示す倒立走行制御を行って走行状態を維持する一方、車体 1 が傾いて地面 800に接触するとき、すなわち、

[数 127]

( > 又は^ < ）の時には、（式 54)に基づく制御を行う。

[0093] ここで、一例として、図 11のように、倒立停止又は走行状態（一点鎖線で示す I)に対して、傾斜位置（点線で示す II又は III)のように 10° 傾斜すると、車体 1が地面 80 0に接触していると仮定するときには、 0.5° の許容値を考慮して、 9.5° 以上傾くと、車体 1が地面 800に接触していると判断するようにすればよい。すなわち、

[数 128] φ>φ,=9.5'>0 又は、

[数 129] ώ<ώ =- .5° <0 のときは、車体 1が地面 800に接触していると判断するようにすればよい。よって、車体 1が傾いて地面 800に接触しないとき、すなわち、

[数 130]

( = 9.5°≥φ≥φ = -9.5Ί のときには、（式 49)で示す倒立走行制御を行って走行状態を維持する一方、車体 1 が傾いて地面 800に接触するとき、すなわち、

[数 131]

( φ > φ_ι, = 9.5°又は（*< =— 9.5° ) の時には、（式 54)に基づく速度制御を行う。

[0094] 以上のように、車体 1の傾斜角度に応じて制御方法を切り替えながら走行することによって、平坦な面において接触が発生しても、暴走することなく安全にかつユーザに負担をかけずに走行を継続できる。

[0095] なお、上記様々な実施形態のうちの任意の実施形態を適宜組み合わせることにより

、それぞれの有する効果を奏するようにすることができる。

産業上の利用可能性

[0096] 本発明に力かる倒立二輪走行型ロボット及びその制御方法では、地面や壁などに接触して回転を拘束された場合でも、姿勢制御や走行状態を維持でき、かつユーザに負担をかけずに、安全に移動することができる倒立二輪走行型ロボット及びその制御方法を提供できる。

[0097] 本発明は、添付図面を参照しながら好ましい実施形態に関連して充分に記載されているが、この技術の熟練した人々にとつては種々の変形や修正は明白である。そのような変形や修正は、添付した請求の範囲による本発明の範囲から外れない限りにおいて、その中に含まれると理解されるべきである。

Claims

請求の範囲

[1] 車体と、

上記車体に同軸に配置された 2つの車輪と、

上記車輪を各々駆動する駆動装置と、

上記車体拘束認識手段の検出結果に基づいて、上記制御手段が、上記複数の走行制御方法を切替える上記駆動装置への指令値を決定し、決定された上記指令値により上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボット。

[2] 上記車体に対する接触力を検出する少なくとも一つの接触センサを上記車体に有するとともに、上記車体拘束認識手段は、上記接触センサで検出された接触情報を入力として、車体拘束状態を認識する請求項 1に記載の倒立二輪走行型ロボット。

[3] 車体拘束時には、上記車輪を回転駆動させるトルクに、上記車体への拘束力による発生するトルク分を加算する制御方法に切り替える請求項 1又は 2に記載の倒立二輪走行型ロボット。

[4] 車体拘束時には上記車体の傾斜角度及び傾斜角速度に対するフィードバックを停止させる制御に切り替える請求項 1又は 2に記載の倒立二輪走行型ロボット。

[5] 車体拘束時には上記車体の傾斜角度及び傾斜角速度に対するフィードバックを停止させ、かつ、上記車体の走行のみを継続する制御に切り替える請求項 4に記載の倒立二輪走行型ロボット。

[6] 車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする請求項 1又は 2に記載の倒立二輪走行型ロボット。

[7] 車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする請求項 3に記載の倒立二輪走行型ロボット。

[8] 車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする請求項 4に記載の倒立二輪走行型ロボット。

[9] 車体拘束時には上記車輪の回転角速度の誤差の積分値をリセットする請求項 5に記載の倒立二輪走行型ロボット。

[10] 車体の傾斜角度及び傾斜角速度の少なくとも 1つを第 1状態検出情報として検出し上記車輪の回転角度及び回転角速度の少なくとも 1つを第 2状態検出情報として検出し、

上記第 1状態検出情報又は上記第 2状態検出情報を入力として、上記車体の傾斜方向への回転が拘束されているかどうかを車体拘束認識情報として検出し、上記検出された車体拘束認識情報に基づ!、て、制御手段が複数の走行制御方法を切替えて、上記車体に同軸に配置された 2つの車輪を各々駆動する駆動装置への指令値を決定し、決定された上記指令値により複数の走行制御方法を切り替えて上記駆動装置が駆動されて、上記車体の走行制御が行われて走行状態を維持する、倒立二輪走行型ロボットの制御方法。