JP5169989B2

JP5169989B2 - 倒立型移動体の制御装置

Info

Publication number: JP5169989B2
Application number: JP2009119768A
Authority: JP
Inventors: 玄安藤
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2009-05-18
Filing date: 2009-05-18
Publication date: 2013-03-27
Anticipated expiration: 2029-05-18
Also published as: JP2010267193A

Description

本発明は、倒立型移動体の制御装置に関する。詳しくは、車輪駆動手段とリンク状の負荷体とを備え、リンク状の負荷体を倒立させるバランス制御を行いながら移動する倒立型移動体の制御装置に関する。

同軸に設けられた左右一対の車輪を有し、倒立状態を維持しながら走行する移動装置が知られている。たとえば、特許文献1（特開2006−123014号公報）には、倒立状態を維持しながら自律的に走行する倒立二輪走行ロボットが開示されている。また、特許文献2（特開2006-315666号公報）には、人が立位姿勢でステップに搭乗した状態でバランスを保ちながら走行する同軸二輪車が開示されている。

図9は、特許文献1に開示された倒立二輪走行ロボットの制御器の構成を示す図である。
図9において、901は摩擦推定器、902は目標状態生成器、903は状態フィードバックゲイン、904は倒立ロボットである。
摩擦推定器901には角速度指令値が入力され、摩擦推定器901は、モータの摩擦および車輪と路面との摩擦を摩擦推定値として算出し出力する。
目標状態生成器902には前記角速度指令値と前記摩擦推定値とが入力され、目標状態生成器902は、制御対象である倒立ロボット904の目標状態を算出し出力する。
状態フィードバックゲイン903には、前記目標状態から倒立ロボット904の状態変数を減算した信号が入力され、状態フィードバックゲイン903は、その入力信号に基づいて倒立ロボット904に所望の動作をさせる状態フィードバック信号を算出して出力する。
倒立ロボット904は、前記摩擦推定値と前記状態フィードバック信号との加算値により駆動される。
このように、従来技術の倒立二輪走行ロボットの制御では、制御対象である倒立ロボット904を所望の姿勢の近傍で線形化した線形化モデルに基づいて倒立ロボット904の動作を制御している。

特開2006-123014号公報（図4）

従来の移動体制御装置では、水平で平らな路面を走行する場合を仮定して２輪倒立の移動体の動作を制御していた。そのため、例えば、路面に凹凸があると、移動体が転倒したり、振動的になったり、所望の水平速度で走行できないなどの問題が発生していた。
また、従来の移動体制御装置では、移動体の所望の姿勢の近傍における線形化モデルに対して制御設計をしていたが、走行中に移動体が人や物体に衝突する場合が起こり得る。
このような場合には非線形のトルクが影響してしまうので、従来の単純な線形化モデルでは対応できず、制御不能になったり、発振したりして転倒するなどの問題が生じていた。

本発明は、このような問題点に鑑みてなされたものであり、路面に凹凸がある場合や、人や物体と衝突した場合にも、転倒することなく、発振することなく２輪倒立の移動体を所望の水平速度で走行させることができる移動体制御装置を提供することを目的とする。

上記問題を解決するため、本発明は、次のように構成したものである。
本発明の倒立型移動体の制御装置は、
車輪を有する駆動手段とリンクを介して前記車輪の上で倒立制御される負荷とを有する移動体本体を倒立させながら走行制御をする制御装置であって、
所望の目標状態を指令する指令部からの指令値と移動体本体の状態を検出する状態センサからの検出信号との偏差に応じて線形制御を行う線形トルクを算出する線形制御部と、
前記状態センサからの検出信号に基づいて移動体本体にかかる非線形要素による非線形トルクを算出する非線形制御部と、
前記線形制御部からの線形トルクと前記非線形制御部からの非線形トルクとにより移動体本体に与えるトルク指令を算出するトルク指令演算器と、を備え
前記非線形制御部は、
前記負荷の重心と前記車輪の重心とをつなぐ直線が鉛直線との間になす角を負荷角度とし、
前記負荷角度を用いて前記移動体本体の車輪水平速度を推定する車輪水平速度推定器と、
前記移動体本体の非線形部分によるトルクである非線形トルクを前記車輪水平速度推定値を用いて算出する非線形トルク演算器と、を備える
ことを特徴とする。

また、本発明の倒立型移動体の制御装置は、
車輪を有する駆動手段とリンクを介して前記車輪の上で倒立制御される負荷とを有する移動体本体を倒立させながら走行制御をする制御装置であって、
所望の目標状態を指令する指令部からの指令値と移動体本体の状態を検出する状態センサからの検出信号との偏差に応じて線形制御を行う線形トルクを算出する線形制御部と、
前記状態センサからの検出信号に基づいて移動体本体にかかる非線形要素による非線形トルクを算出する非線形制御部と、
前記線形制御部からの線形トルクと前記非線形制御部からの非線形トルクとにより移動体本体に与えるトルク指令を算出するトルク指令演算器と、を備え
前記非線形制御部は、
前記負荷の重心と前記車輪の重心とをつなぐ直線が鉛直線との間になす角を負荷角度とし、
前記負荷角度を用いて前記移動体本体の車輪垂直加速度を推定する車輪垂直加速度推定器と、
前記移動体本体の非線形部分によるトルクである非線形トルクを前記車輪垂直加速度推定値を用いて算出する非線形トルク演算器と、を備える
ことを特徴とする。

このような本発明によれば、非線形要素を加味した制御により、路面に凹凸がある場合や、人や物体と衝突した場合にも、転倒することなく、発振することなく２輪倒立の移動体を所望の水平速度で走行させることができる。

本発明の倒立型移動体に係る第１実施形態を示す図。移動体本体をモデル化した図。負荷角度のシミュレーション結果を示す図。車輪水平速度のシミュレーション結果を示す図。変形例1を示す図。倒立型移動体として同軸二輪車を示す図。倒立型移動体として倒立型自律走行ロボットを示す図。 4輪の車輪駆動手段の上に揺動可能にリンク機構を備えた倒立型移動体を示す図。従来の倒立二輪走行ロボットの制御器の構成を示す図。

以下、本発明の実施の形態について図を参照して説明する。
（第1実施形態）

図1は、本発明の倒立型移動体に係る第１実施形態を示す図である。
倒立型移動体100は、制御対象としての移動体本体112と、移動体本体112の状態を検出する状態センサ113と、所望の目標状態を指令する指令部120と、状態センサ113による検出結果と指令部120からの指令値とに基づいて移動体本体112の制御を実行する制御装置130と、を備える。

移動体本体112としては、同軸二輪車（図6）、倒立型自律走行ロボット（図7）などが一般的な例として挙げられる。
これに限らず、車輪による駆動手段とリンク状の負荷体とを備え、リンク状の負荷体を倒立させるバランス制御を行うものであればよい。
たとえば、図8に示すような構成でもよい。
図8は、4輪の車輪駆動手段801の上に揺動可能にリンク機構802が設けられている構成である。
たとえば、リンク機構802の上部をカゴ状803にして、物品を載せて運ぶようにしてもよい。

そして、以下の説明では、このような移動体本体112を、図2のようにモデル化する。
ここで、図2において、201は負荷、202は車輪、203は路面である。
図2に示すように移動体本体112は倒立して走行するものとする。
負荷201は、ロボットのボディや、移動体本体112に乗る人または荷物である。
車輪202は、負荷201を乗せ、回転することにより路面203との摩擦力を利用して移動する。

状態センサ113は、負荷201の角度（θ₁）と、車輪202の角度（θ₂）を検出するものである。

指令部120は、車輪水平速度指令生成器121と、負荷角度指令演算器122と、を備える。
車輪水平速度指令生成器121は、移動体本体112の車輪202の所望の水平移動速度である車輪水平速度指令を生成し出力する。
負荷角度指令演算器122は、前記車輪水平速度指令を入力とし、移動体本体112が走行する路面203が水平な場合に車輪水平速度が前記車輪水平速度指令に追従するような負荷角度である負荷角度指令を算出し出力する。

制御装置130は、場合分け線形制御部140と、非線形制御部150と、トルク指令演算器111と、を備える。

場合分け線形制御部140は、減衰範囲演算器141と、減衰パラメータ演算器142と、場合分け線形トルク演算器143と、制御切替器144と、を備える。

減衰範囲演算器141には、負荷角度指令演算器122からの負荷角度指令と、状態センサ113による検出値である負荷角度（θ₁）および車輪角度（θ₂）と、が入力される。
減衰範囲演算器141は、入力信号に基づき、移動体本体112の動作制御において粘性摩擦を用いた減衰のみ加える負荷角度の範囲を減衰範囲として算出し出力する。

減衰パラメータ演算器142には、負荷角度指令演算器122からの負荷角度指令（θ₁ ^＊）と、状態センサ113による検出値である負荷角度（θ₁）および車輪角度（θ₂）と、が入力される。減衰パラメータ演算器142は、入力信号に基づき、前記減衰範囲における制御に用いる減衰パラメータを算出し出力する。

場合分け線形トルク演算器143には、負荷角度指令演算器122からの負荷角度指令（θ₁ ^＊）と、減衰パラメータ演算器142からの減衰パラメータと、状態センサ113による検出値である負荷角度（θ₁）および車輪角度（θ₂）と、が入力される。場合分け線形トルク演算器143は、負荷速度と減衰パラメータとの乗算値に負号を付した減衰トルクと、位置偏差、速度偏差および加速度偏差の一つ以上に所定ゲインを乗算して得る線形フィードバックトルクと、を算出して出力する。

制御切替器144には、減衰範囲演算器141にて算出された減衰範囲と、状態センサ113による検出値と、場合分け線形トルク演算器143にて算出された場合分け線形トルクと、が入力されている。制御切替器144は、場合分け線形トルク演算器143にて算出された場合分け線形トルクを切り替えて出力する。

非線形制御部150は、車輪垂直加速度推定器151と、車輪水平速度推定器152と、非線形トルク演算器153と、を備える。
車輪垂直加速度推定器151には、状態センサ113からの検出信号が入力され、車輪垂直加速度推定器151は、その入力信号に基づいて車輪202の垂直加速度を推定し車輪垂直加速度推定値として出力する。

車輪水平速度推定器152には、状態センサ113からの検出信号が入力されているとともに、トルク指令演算器111からのトルク指令が分岐して入力されている。車輪水平速度推定器152は、その入力信号に基づいて車輪202の水平速度を推定し車輪水平速度推定値として出力する。
非線形トルク演算器153には、前記車輪垂直加速度推定値と前記車輪水平速度推定値とが入力され、非線形トルク演算器153は、移動体本体112の非線形ダイナミクスを示す非線形トルクを演算し出力する。

トルク指令演算器111には、制御切替器144で切り替え出力される前記場合分け線形トルクと非線形トルク演算器153から出力される前記非線形トルクとが入力され、トルク指令演算器111は、これら入力信号の加算値を車輪202の半径で除算して得たトルク指令を出力する。

移動体本体112は前記トルク指令により駆動される。

以下、本第1実施形態に係る制御装置が移動体本体112の動作を制御する仕組みの詳細を説明する。

図2において、次のようにパラメータを設定する。
ｍ_１は負荷質量、
Ｊ_１は負荷慣性モーメント、
ｍ_２は車輪質量、
Ｊ_２は車輪慣性モーメント、
ｌは負荷と車輪との重心間距離である負荷車輪重心間距離、
rは車輪半径、
θ_１は負荷角度、
θ_２は車輪角度、
Ｔ_ｒｅｆはトルク指令、とする。
さらに、車輪水平位置をｘ_２、車輪垂直位置をｙ_２とすると、負荷水平位置ｘ_１および負荷垂直位置ｙ_１は次のようにそれぞれ式（１）と式（２）とであらわされる。

式（１）と式（２）とを用い、移動体本体112の運動エネルギーＴおよびポテンシャルエネルギーＶは次のようにそれぞれ式（３）と式（４）とであらわされる。

すると、オイラーラグランジュ方程式を用い、移動体本体112の運動方程式は式（５）乃至（８）と求められる。

ただし、ｇは重力加速度である。
さらに、車輪202と路面203の間の粘性摩擦を考慮すると、式（６）と式（７）は式（９）と式（１０）と書き換えられる。
ここに、Dは粘性摩擦係数とする。

式（９）と式（１０）とを用い、式（１１）が得られる。

式（５）から式（１１）を減算すると式（１２）が得られる。

ただし、N_xは車輪水平位置x₂をパラメータとした非線形項、N_yは車輪垂直位置y₂をパラメータとした非線形項である。

負荷角度θ₁は車輪水平位置x₂より十分に遅く変化すると仮定すると、式（１１）は式（１３）と書き換えられる。

ただし、車輪水平位置x₂より十分に遅く変化する部分を定数c₁、c₂、c₃とした。
式（１３）より、車輪水平速度推定値は式（１４）であらわされる。

sはラプラス演算子、L^−１はラプラス逆変換、l₁、l₂は車輪水平速度推定器152のパラメータを表す。
車輪水平速度推定器152は式（１４）を用いて前記車輪水平速度推定値を算出する。

一方、式（８）を車輪垂直加速度（d²y₂／dt²）について解くと式（１５）が得られる。

車輪垂直加速度推定器151は、式（１５）を用いて前記車輪垂直加速度推定値を算出する。

所望の車輪水平速度である車輪水平速度指令をv₂ ^＊（＝x₂*の一階微分）とすると、路面203が水平で平らな場合に車輪水平速度dx₂／dtが前記車輪水平速度指令v₂*となる負荷角度θ₁である負荷角度指令θ₁ ^＊は式（１６）であらわされる。すなわち、負荷角度指令は、車輪水平加速度指令を重力加速度で除算した値の逆正接である。

車輪水平速度指令生成器121は、車輪水平速度指令v₂ ^＊（＝x₂*の一階微分）を出力し、負荷角度指令演算器122は、式（１６）を用いて負荷角度指令θ₁ ^＊を算出し出力する。

式（１４）と式（１５）を式（１２）に代入すると、式（１７）が得られる。

式（１７）を書き換えると式（１８）とあらわされる。

ただし、ｕは場合分け線形トルクである。
そして、負荷角度θ₁が負荷角度指令θ₁ ^＊に収束するような、式（１９）に示す場合分け線形トルクｕを考える。

ただし、ｅ＝θ₁ ^＊−θ₁は、負荷角度追従偏差、
βは、速度比例制御ゲイン、
κは、位置比例制御ゲイン、
γは、減衰パラメータ、
ｓｇｎ（・）は、・が正数のとき＋１、負数のとき−１、零のとき０の値をとるシグナム関数である。

また、ｈ＝ｃ｜θ₁ ^＊｜は、フィードバック制御によるチャタリングを抑制するために設けた負荷角度範囲である減衰範囲であり、ｃは、減衰範囲ｈのパラメータである。

ここで、式（１９）に表される場合分け線形トルクｕの意味は、負荷角度θ₁と負荷角度指令θ₁ ^＊との偏差が小さい場合、具体的には、負荷角度θ₁の絶対値が減少する方向において負荷角度θ₁と負荷角度指令θ₁ ^＊との角度ずれが減衰範囲ｈの範囲内である場合、負荷201には粘性摩擦が減衰パラメータγである動作をさせることにより、負荷角度θ₁を負荷角度指令θ₁ ^＊に安定して収束させることができることを表す。
また、負荷角度θ₁と負荷角度指令θ₁ ^＊とのずれが上記以外の場合、負荷201には、剛性が位置比例制御ゲインκで、粘性摩擦が速度比例制御ゲインβであるフィードバック制御をかけることにより、負荷角度θ₁を負荷角度指令θ₁ ^＊に収束させることができることを表す。

ここで、減衰パラメータγを例えば式（２０）に示すように負荷角度指令θ₁ ^＊と負荷角度追従偏差ｅの関数として設定すると好適である。

すなわち、場合分け線形制御演算器140は、負荷角度指令演算器122から負荷角度指令θ₁ ^＊を受けると、負荷角度指令θ₁ ^＊は減衰範囲演算器141、減衰パラメータ演算器142および場合分け線形トルク演算器143に入力される。
そして、減衰範囲演算器141は、負荷角度指令θ₁ ^＊とパラメータcとを用いて、ｈ＝ｃ｜θ₁ ^＊｜である減衰範囲を算出する。
この求められた減衰範囲hは制御切替器144に出力される。

また、減衰パラメータ演算器142は、上記式（20）に従って減衰パラメータγを算出し、場合分け線形トルク演算器143に出力する。

場合分け線形トルク演算器143は、減衰パラメータ演算器142から与えられる減衰パラメータγおよび予め設定された速度比例制御ゲインβ、位置比例制御ゲインκを用い、式（19）の場合分け線形トルクuを算出する。
算出した場合分け線形トルクuは、制御切替器144に出力する。

制御切替器144は、場合分け線形トルク演算器143にて求められた場合分け線形トルクuを負荷角度追従偏差ｅおよび減衰範囲hを参照して切替選択する。
制御切替器144にて選択された場合分け線形トルクはトルク指令演算器111に出力される。

また、非線形トルク演算器153は、式（１５）を用いて算出した車輪垂直加速度推定値と、式（１４）を用いて算出した車輪水平速度推定値に基づいて、式（１２）のＮｘ＋Ｎｙである非線形トルクを算出し出力する。

トルク指令演算器111は、制御切替値ｕと非線形トルクＮｘ＋Ｎｙを用いて式（２１）によりトルク指令Ｔｒｅｆを算出し出力する。

ただし、rは車輪半径である。
移動体本体112はトルク指令T_refにより駆動制御される。

このような構成を備える本第1実施形態によれば、次の効果を奏することができる。
（１）本実施形態では、式（１４）の車輪水平速度推定値を用いて移動体本体112を制御するので、車輪202と路面203が相対的にすべる場合にも移動体本体112の車輪水平速度v₂（＝x₂の一階微分）を車輪水平速度指令v₂ ^＊（x₂*の一階微分）に一致させることができる。

従来は、単純な線形モデルを想定した制御設計を採用していたため、車輪と路面とがすべるような非線形要素に対して特別な考慮をしていなかった。
そして、従来は、車輪のすべりのような非線形要素も単なる外乱の一つとして押さえこむようにゲイン等を設定していたが、路面との滑りや衝突の影響で車輪が空転し、負荷の姿勢が大きくずれるような場合にはロバスト性を考慮した制御設計であっても安定状態に復帰させるには無理がある。
この点、本実施形態では、式（１４）の車輪水平速度推定値を車輪水平速度推定器152によって推定し、非線形トルク演算器153において、式（12）の車輪水平位置x₂をパラメータとする非線形項Nxを求めて、これをトルク指令に加えている。したがって、車輪202と路面203が相対的にすべるような非線形要素にも対応し、移動体本体112の車輪水平速度v₂（＝x₂の一階微分）を車輪水平速度指令v₂ ^＊（x₂*の一階微分）に一致させることができる。

（２）本実施形態では、式（１５）の車輪垂直加速度推定値を用いて移動体本体112を制御するので、路面203に凹凸があっても車輪202が路面203に接触している間は負荷角度θ₁を安定に制御することができる。
従来は水平な路面を走行する場合を仮定しており、凹凸を乗り越えるような非線形要素を考慮していなかった。そのため、路面凹凸を乗り越える際に負荷が大きく傾くような場合には発振したり、制御不能になって転倒するなどの問題があった。
この点、本実施形態では、式（15）の車輪垂直加速度推定値を車輪垂直加速度推定器151によって算出し、非線形トルク演算器153において、式（12）の車輪垂直位置y2をパラメータとする非線形項Nyを求めて、これをトルク指令に加えている。
したがって、路面凹凸を乗り越えるような非線形要素にも対応し、負荷角度θ1を安定に制御することができる。

（3）本第1実施形態では、式（１９）に示すように、負荷角度θ₁を３つの範囲に分け、それぞれの場合に最適なトルク指令を算出する。
そして、制御切替器144において減衰範囲の内と外とで制御を切り替える。
これにより、負荷角度θ₁が負荷角度指令θ₁ ^＊の近傍において振動することなく、負荷角度指令θ₁ ^＊に滑らかに収束するようにすることができる。
その結果、安定した水平走行を実現させることができる。

（4）減衰パラメータ演算器142を備え、式（２０）により減衰パラメータγを設定するので、減衰パラメータγを一定値に固定する場合に比べ、滑らかに速く負荷角度θ₁を負荷角度指令θ₁ ^＊に収束させることができる。

（実験例）
次に、本発明の効果を実証する実験例を示す。
実験例として、第１実施形態のシミュレーション結果を示す。
ここで、シミュレーションに用いた数値は以下のとおりである。

m₁=70 [kg]、
J₁= 25.2 [kg・m²]、
m₂= 15 [kg]、
J₂= 0.075 [kg・m²]、
l = 0.9 [m]、
r = 0.1 [m]、
D = 0.1 [N・s／m]、
g = 9.8 [m／s²]、
T = 1×10^-3 [ｓ]、
κ = 40 [s^-1]、
J₁₀= m₁×l²＋J₁ [kg・m²]、
β₀= 2πκ [s^-1]、
β = β₀×J₁₀ [N・m・s／rad]、
γ = 0.1 [N・m・s／rad]、
pcl = [-49.9、-201.4］ [rad／s]、
td = 0.5 [s]

ただし、
m₁は負荷質量、
J₁は、負荷慣性モーメント、
m₂は、車輪質量、
J₂は、車輪慣性モーメント、
lは、負荷車輪重心間距離、
rは、車輪半径、
Dは、車輪路面間粘性摩擦、
gは、重力加速度、
Tは、制御周期、
κは、本発明における位置比例制御ゲイン、
J₁₀は、公称慣性モーメント、
β₀は、本発明における正規化速度比例制御ゲイン、
βは、本発明における速度比例制御ゲイン、
γは、本発明における摩擦パラメータ、
pclは、従来技術における閉ループ極、
tdは、インパルス外乱時間、である。

なお、
車輪路面間粘性摩擦Dは、図2の車輪202と路面203の間にはたらく粘性摩擦であり、
公称慣性モーメントJ₁₀は、本発明における速度ループを正規化するパラメータであり、
pclは、従来技術における状態フィードバック制御で配置する閉ループの極、である。
また、インパルス外乱時間tdにおいて、車輪202の鉛直上方方向にインパルス状の加速度外乱入力がある場合を考える。

図3、図4はシミュレーション結果を示す図である。
図3は、負荷角度の変化を示す。
図3において、破線L₁₁は負荷角度指令（Load angular position reference input）、実線L₁₀は本発明による負荷角度（Load angular position with proposed control）、一点鎖線L₁₂は従来技術による負荷角度を示す。
加速度外乱を入力する0.5[s]までは、従来技術と本発明とは共に負荷角度指令にほぼ同じ程度に追従しているが、0.5[s]以降は従来技術では負荷角度が発振しているのに対し、本発明では前記加速度外乱入力後も発振することなく前記負荷角度指令に追従していることが分かる。

そして、本発明による負荷角度が折れ線状の時間変化をしているのは、式（１９）の前記減衰範囲において減衰のみを加える制御を実施しているためであり、前記減衰範囲をもたせることにより前記負荷位置が前記負荷位置指令の近傍において振動的になりにくくなっていることが示された。

また、車輪垂直加速度推定器151において、式（１５）の車輪垂直加速度推定値を算出し、非線形トルク演算器153において、非線形トルクNyを算出することにより、路面203の凹凸のため車輪202に加わる垂直方向の前記加速度外乱を補償できるので、本発明は前記加速度外乱が存在する場合にも前記負荷角度を安定化させることが示された。

図4は、車輪の水平速度変化を示す。
図4において、破線L₂₁は所望の車輪水平速度、実線L₂₀は本発明による車輪水平速度、一点鎖線L₂₂は従来技術による車輪水平速度である。
加速度外乱を入力する0.5[s]までは従来技術も本発明も共に前記所望の車輪水平速度に追従しているが、0.5[s]以降において従来技術は振動的となるのに対し、本発明は振動的とならず前記所望の車輪水平速度に追従していることがわかる。

（変形例1）
上記第1実施形態においては、最も好適な実施形態として場合分け線形制御部140を有する場合を説明したが、図5に示すように、場合分け線形制御部に代えて線形トルク演算器501とし、従来の線形フィードバック制御を適用してもよい。
この場合でも、非線形制御部150を有するので、車輪のすべり、衝突、路面凹凸などの非線形要素があってもこれらに対応し、安定した走行を実現できる。

なお、本発明は上記実施形態に限定されず、本発明の目的を達成できる範囲での変形、改良等は本発明に含まれる。
例えば、式（１９）において、位置Ｐ／速度Ｐ制御は、位置Ｐ／速度ＰＩ制御、位置Ｐ／速度Ｉ−Ｐ制御、位置ＰＩＤ制御など、任意の制御則に置き換えても良いことはもちろんである。

本発明は、路面に凹凸がある場合にも、人や物体と衝突した場合にも、転倒することなく、発振することなく２輪倒立の移動体を所望の水平速度で走行させることができるので、２輪倒立で走行するロボット、電動車椅子、自動搬送装置、災害時の人命救助など狭い場所で作業するロボット、振動に弱い電子部品の組み立て装置などに広く適用できる。

100…倒立型移動体、111…トルク指令演算器、112…移動体本体、113…状態センサ、120…指令部、121…車輪水平速度指令生成器、122…負荷角度指令演算器、130…制御装置、140…場合分け線形制御部、141…減衰範囲演算器、142…減衰パラメータ演算器、144…制御切替器、150…非線形制御部、151…車輪垂直加速度推定器、152…車輪水平速度推定器、153…非線形トルク演算器、201…負荷、202…車輪、203…路面、901…摩擦推定器、902…目標状態生成器、903…状態フィードバックゲイン、904…倒立ロボット。

Claims

車輪を有する駆動手段とリンクを介して前記車輪の上で倒立制御される負荷とを有する移動体本体を倒立させながら走行制御をする制御装置であって、
所望の目標状態を指令する指令部からの指令値と移動体本体の状態を検出する状態センサからの検出信号との偏差に応じて線形制御を行う線形トルクを算出する線形制御部と、
前記状態センサからの検出信号に基づいて移動体本体にかかる非線形要素による非線形トルクを算出する非線形制御部と、
前記線形制御部からの線形トルクと前記非線形制御部からの非線形トルクとにより移動体本体に与えるトルク指令を算出するトルク指令演算器と、を備え
前記非線形制御部は、
前記負荷の重心と前記車輪の重心とをつなぐ直線が鉛直線との間になす角を負荷角度とし、
前記負荷角度を用いて前記移動体本体の車輪水平速度を推定する車輪水平速度推定器と、
前記移動体本体の非線形部分によるトルクである非線形トルクを前記車輪水平速度推定値を用いて算出する非線形トルク演算器と、を備える
ことを特徴とする倒立型移動体の制御装置。
請求項1に記載の倒立型移動体の制御装置において、
前記車輪水平速度推定器は、次の式によって車輪水平速度推定値を算出することを特徴とする倒立型移動体の制御装置。

ただし、

ここで、
θ₁は負荷角度であり、
ｍ_１は負荷質量であり、
ｍ_２は車輪質量であり、
ｌは負荷と車輪との重心間距離である負荷車輪重心間距離であり、
rは車輪半径であり、
θ_１は負荷角度であり、
Ｔ_ｒｅｆはトルク指令であり、
ｓはラプラス演算子であり、
l₁、l₂は車輪水平速度推定器のパラメータであり、
L^−１はラプラス逆変換を表す。
請求項2に記載の倒立型移動体の制御装置において、
前記非線形トルク演算器は、次の式によって車輪水平位置x₂をパラメータとした非線形項N_xを算出する
ことを特徴とする倒立型移動体の制御装置。
車輪を有する駆動手段とリンクを介して前記車輪の上で倒立制御される負荷とを有する移動体本体を倒立させながら走行制御をする制御装置であって、
所望の目標状態を指令する指令部からの指令値と移動体本体の状態を検出する状態センサからの検出信号との偏差に応じて線形制御を行う線形トルクを算出する線形制御部と、
前記状態センサからの検出信号に基づいて移動体本体にかかる非線形要素による非線形トルクを算出する非線形制御部と、
前記線形制御部からの線形トルクと前記非線形制御部からの非線形トルクとにより移動体本体に与えるトルク指令を算出するトルク指令演算器と、を備え
前記非線形制御部は、
前記負荷の重心と前記車輪の重心とをつなぐ直線が鉛直線との間になす角を負荷角度とし、
前記負荷角度を用いて前記移動体本体の車輪垂直加速度を推定する車輪垂直加速度推定器と、
前記移動体本体の非線形部分によるトルクである非線形トルクを前記車輪垂直加速度推定値を用いて算出する非線形トルク演算器と、を備える
ことを特徴とする倒立型移動体の制御装置。
請求項4に記載の倒立型移動体の制御装置において、
前記車輪垂直加速度推定器は、次の式によって車輪垂直加速度推定値を算出する
ことを特徴とする倒立型移動体の制御装置。

ただし、
θ₁は負荷角度であり、
ｍ_１は負荷質量であり、
ｍ_２は車輪質量であり、
ｌは負荷と車輪との重心間距離である負荷車輪重心間距離である。
請求項5に記載の倒立型移動体の制御装置において、
前記非線形トルク演算器は、次の式によって車輪垂直位置y₂をパラメータとした非線形項N_yを算出することを特徴とする倒立型移動体の制御装置。

θ₁は負荷角度であり、
ｍ_１は負荷質量であり、
ｍ_２は車輪質量であり、
ｌは負荷と車輪との重心間距離である負荷車輪重心間距離であり、
rは車輪半径であり、
Ｊ_２は車輪慣性モーメントであり、
θ_２は車輪角度である。
請求項1から請求項6のいずれかに記載の倒立型移動体の制御装置において、
前記トルク指令演算器は、前記線形制御部からの線形トルクと前記非線形制御部からの非線形トルクとを加算した加算値を移動体本体の車輪半径で除算した値をトルク指令とする
ことを特徴とする倒立型移動体の制御装置。