(57)【要約】【課題】非線形要素を加味した制御により、車輪の空転、衝突、路面凹凸などに対応できる倒立型の移動体を提供する。【解決手段】状態センサ１１３からの検出信号に基づいて移動体本体１１２にかかる非線形要素による非線形トルクを算出する非線形制御部１５０を備える。トルク指令演算器１１１は、線形制御部１４０からの線形トルクと非線形制御部１５０からの非線形トルクとにより移動体本体１１２に与えるトルク指令を算出する。【選択図】図１
2010267193.tif 000002
【特許請求の範囲】
【請求項１】
車輪を有する駆動手段とリンクを介して前記車輪の上で倒立制御される負荷とを有する移動体本体を倒立させながら走行制御をする制御装置であって、
所望の目標状態を指令する指令部からの指令値と移動体本体の状態を検出する状態センサからの検出信号との偏差に応じて線形制御を行う線形トルクを算出する線形制御部と、
前記状態センサからの検出信号に基づいて移動体本体にかかる非線形要素による非線形トルクを算出する非線形制御部と、
前記線形制御部からの線形トルクと前記非線形制御部からの非線形トルクとにより移動体本体に与えるトルク指令を算出するトルク指令演算器と、を備え
前記非線形制御部は、
前記負荷の重心と前記車輪の重心とをつなぐ直線が鉛直線との間になす角を負荷角度とし、
前記負荷角度を用いて前記移動体本体の車輪水平速度を推定する車輪水平速度推定器と、
前記移動体本体の非線形部分によるトルクである非線形トルクを前記車輪水平速度推定値を用いて算出する非線形トルク演算器と、を備える
ことを特徴とする倒立型移動体の制御装置。
【請求項２】
請求項1に記載の倒立型移動体の制御装置において、
前記車輪水平速度推定器は、次の式によって車輪水平速度推定値を算出することを特徴とする倒立型移動体の制御装置。
2010267193.tif 000024
ただし、
2010267193.tif 000025
ここで、
θ1は負荷角度であり、
ｍ１は負荷質量であり、
ｍ２は車輪質量であり、
ｌは負荷と車輪との重心間距離である負荷車輪重心間距離であり、
rは車輪半径であり、
θ１は負荷角度であり、
Ｔｒｅｆはトルク指令であり、
ｓはラプラス演算子であり、
l1、l2は車輪水平速度推定器のパラメータであり、
L−１はラプラス逆変換を表す。
【請求項３】
請求項2に記載の倒立型移動体の制御装置において、
前記非線形トルク演算器は、次の式によって車輪水平位置x2をパラメータとした非線形項Nxを算出する
ことを特徴とする倒立型移動体の制御装置。
2010267193.tif 000026
【請求項４】
車輪を有する駆動手段とリンクを介して前記車輪の上で倒立制御される負荷とを有する移動体本体を倒立させながら走行制御をする制御装置であって、
所望の目標状態を指令する指令部からの指令値と移動体本体の状態を検出する状態センサからの検出信号との偏差に応じて線形制御を行う線形トルクを算出する線形制御部と、
前記状態センサからの検出信号に基づいて移動体本体にかかる非線形要素による非線形トルクを算出する非線形制御部と、
前記線形制御部からの線形トルクと前記非線形制御部からの非線形トルクとにより移動体本体に与えるトルク指令を算出するトルク指令演算器と、を備え
前記非線形制御部は、
前記負荷の重心と前記車輪の重心とをつなぐ直線が鉛直線との間になす角を負荷角度とし、
前記負荷角度を用いて前記移動体本体の車輪垂直加速度を推定する車輪垂直加速度推定器と、
前記移動体本体の非線形部分によるトルクである非線形トルクを前記車輪垂直加速度推定値を用いて算出する非線形トルク演算器と、を備える
ことを特徴とする倒立型移動体の制御装置。
【請求項５】
請求項4に記載の倒立型移動体の制御装置において、
前記車輪垂直加速度推定器は、次の式によって車輪垂直加速度推定値を算出する
ことを特徴とする倒立型移動体の制御装置。
2010267193.tif 000027
ただし、
θ1は負荷角度であり、
ｍ１は負荷質量であり、
ｍ２は車輪質量であり、
ｌは負荷と車輪との重心間距離である負荷車輪重心間距離である。
【請求項６】
請求項5に記載の倒立型移動体の制御装置において、
前記非線形トルク演算器は、次の式によって車輪垂直位置y2をパラメータとした非線形項Nyを算出することを特徴とする倒立型移動体の制御装置。
2010267193.tif 000028
θ1は負荷角度であり、
ｍ１は負荷質量であり、
ｍ２は車輪質量であり、
ｌは負荷と車輪との重心間距離である負荷車輪重心間距離であり、
rは車輪半径であり、
Ｊ２は車輪慣性モーメントであり、
θ２は車輪角度である。
【請求項７】
請求項1から請求項6のいずれかに記載の倒立型移動体の制御装置において、
前記トルク指令演算器は、前記線形制御部からの線形トルクと前記非線形制御部からの非線形トルクとを加算した加算値を移動体本体の車輪半径で除算した値をトルク指令とする
ことを特徴とする倒立型移動体の制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、倒立型移動体の制御装置に関する。詳しくは、車輪駆動手段とリンク状の負荷体とを備え、リンク状の負荷体を倒立させるバランス制御を行いながら移動する倒立型移動体の制御装置に関する。
【背景技術】
【０００２】
同軸に設けられた左右一対の車輪を有し、倒立状態を維持しながら走行する移動装置が知られている。たとえば、特許文献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の角度（θ1）と、車輪202の角度（θ2）を検出するものである。
【００１６】
指令部120は、車輪水平速度指令生成器121と、負荷角度指令演算器122と、を備える。
車輪水平速度指令生成器121は、移動体本体112の車輪202の所望の水平移動速度である車輪水平速度指令を生成し出力する。
負荷角度指令演算器122は、前記車輪水平速度指令を入力とし、移動体本体112が走行する路面203が水平な場合に車輪水平速度が前記車輪水平速度指令に追従するような負荷角度である負荷角度指令を算出し出力する。
【００１７】
制御装置130は、場合分け線形制御部140と、非線形制御部150と、トルク指令演算器111と、を備える。
【００１８】
場合分け線形制御部140は、減衰範囲演算器141と、減衰パラメータ演算器142と、場合分け線形トルク演算器143と、制御切替器144と、を備える。
【００１９】
減衰範囲演算器141には、負荷角度指令演算器122からの負荷角度指令と、状態センサ113による検出値である負荷角度（θ1）および車輪角度（θ2）と、が入力される。
減衰範囲演算器141は、入力信号に基づき、移動体本体112の動作制御において粘性摩擦を用いた減衰のみ加える負荷角度の範囲を減衰範囲として算出し出力する。
【００２０】
減衰パラメータ演算器142には、負荷角度指令演算器122からの負荷角度指令（θ1＊）と、状態センサ113による検出値である負荷角度（θ1）および車輪角度（θ2）と、が入力される。減衰パラメータ演算器142は、入力信号に基づき、前記減衰範囲における制御に用いる減衰パラメータを算出し出力する。
【００２１】
場合分け線形トルク演算器143には、負荷角度指令演算器122からの負荷角度指令（θ1＊）と、減衰パラメータ演算器142からの減衰パラメータと、状態センサ113による検出値である負荷角度（θ1）および車輪角度（θ2）と、が入力される。場合分け線形トルク演算器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は車輪半径、
θ１は負荷角度、
θ２は車輪角度、
Ｔｒｅｆはトルク指令、とする。
さらに、車輪水平位置をｘ２、車輪垂直位置をｙ２とすると、負荷水平位置ｘ１および負荷垂直位置ｙ１は次のようにそれぞれ式（１）と式（２）とであらわされる。
【００２９】
2010267193.tif 000003
【００３０】
2010267193.tif 000004
【００３１】
式（１）と式（２）とを用い、移動体本体112の運動エネルギーＴおよびポテンシャルエネルギーＶは次のようにそれぞれ式（３）と式（４）とであらわされる。
【００３２】
2010267193.tif 000005
【００３３】
2010267193.tif 000006
【００３４】
すると、オイラーラグランジュ方程式を用い、移動体本体112の運動方程式は式（５）乃至（８）と求められる。
【００３５】
2010267193.tif 000007
【００３６】
2010267193.tif 000008
【００３７】
2010267193.tif 000009
【００３８】
2010267193.tif 000010
【００３９】
ただし、ｇは重力加速度である。
さらに、車輪202と路面203の間の粘性摩擦を考慮すると、式（６）と式（７）は式（９）と式（１０）と書き換えられる。
ここに、Dは粘性摩擦係数とする。
【００４０】
2010267193.tif 000011
【００４１】
2010267193.tif 000012
【００４２】
式（９）と式（１０）とを用い、式（１１）が得られる。
【００４３】
2010267193.tif 000013
【００４４】
式（５）から式（１１）を減算すると式（１２）が得られる。
【００４５】
2010267193.tif 000014
【００４６】
ただし、Nxは車輪水平位置x2をパラメータとした非線形項、Nyは車輪垂直位置y2をパラメータとした非線形項である。
【００４７】
負荷角度θ1は車輪水平位置x2より十分に遅く変化すると仮定すると、式（１１）は式（１３）と書き換えられる。
【００４８】
2010267193.tif 000015
【００４９】
ただし、車輪水平位置x2より十分に遅く変化する部分を定数c1、c2、c3とした。
式（１３）より、車輪水平速度推定値は式（１４）であらわされる。
【００５０】
2010267193.tif 000016
【００５１】
sはラプラス演算子、L−１はラプラス逆変換、l1、l2は車輪水平速度推定器152のパラメータを表す。
車輪水平速度推定器152は式（１４）を用いて前記車輪水平速度推定値を算出する。
【００５２】
一方、式（８）を車輪垂直加速度（d2y2／dt2）について解くと式（１５）が得られる。
【００５３】
2010267193.tif 000017
【００５４】
車輪垂直加速度推定器151は、式（１５）を用いて前記車輪垂直加速度推定値を算出する。
【００５５】
所望の車輪水平速度である車輪水平速度指令をv2＊（＝x2*の一階微分）とすると、路面203が水平で平らな場合に車輪水平速度dx2／dtが前記車輪水平速度指令v2*となる負荷角度θ1である負荷角度指令θ1＊は式（１６）であらわされる。すなわち、負荷角度指令は、車輪水平加速度指令を重力加速度で除算した値の逆正接である。
【００５６】
2010267193.tif 000018
【００５７】
車輪水平速度指令生成器121は、車輪水平速度指令v2＊（＝x2*の一階微分）を出力し、負荷角度指令演算器122は、式（１６）を用いて負荷角度指令θ1＊を算出し出力する。
【００５８】
式（１４）と式（１５）を式（１２）に代入すると、式（１７）が得られる。
【００５９】
2010267193.tif 000019
【００６０】
式（１７）を書き換えると式（１８）とあらわされる。
【００６１】
2010267193.tif 000020
【００６２】
ただし、ｕは場合分け線形トルクである。
そして、負荷角度θ1が負荷角度指令θ1＊に収束するような、式（１９）に示す場合分け線形トルクｕを考える。
【００６３】
2010267193.tif 000021
【００６４】
ただし、ｅ＝θ1＊−θ1は、負荷角度追従偏差、
βは、速度比例制御ゲイン、
κは、位置比例制御ゲイン、
γは、減衰パラメータ、
ｓｇｎ（・）は、・が正数のとき＋１、負数のとき−１、零のとき０の値をとるシグナム関数である。
【００６５】
また、ｈ＝ｃ｜θ1＊｜は、フィードバック制御によるチャタリングを抑制するために設けた負荷角度範囲である減衰範囲であり、ｃは、減衰範囲ｈのパラメータである。
【００６６】
ここで、式（１９）に表される場合分け線形トルクｕの意味は、負荷角度θ1と負荷角度指令θ1＊との偏差が小さい場合、具体的には、負荷角度θ1の絶対値が減少する方向において負荷角度θ1と負荷角度指令θ1＊との角度ずれが減衰範囲ｈの範囲内である場合、負荷201には粘性摩擦が減衰パラメータγである動作をさせることにより、負荷角度θ1を負荷角度指令θ1＊に安定して収束させることができることを表す。
また、負荷角度θ1と負荷角度指令θ1＊とのずれが上記以外の場合、負荷201には、剛性が位置比例制御ゲインκで、粘性摩擦が速度比例制御ゲインβであるフィードバック制御をかけることにより、負荷角度θ1を負荷角度指令θ1＊に収束させることができることを表す。
【００６７】
ここで、減衰パラメータγを例えば式（２０）に示すように負荷角度指令θ1＊と負荷角度追従偏差ｅの関数として設定すると好適である。
【００６８】
2010267193.tif 000022
【００６９】
すなわち、場合分け線形制御演算器140は、負荷角度指令演算器122から負荷角度指令θ1＊を受けると、負荷角度指令θ1＊は減衰範囲演算器141、減衰パラメータ演算器142および場合分け線形トルク演算器143に入力される。
そして、減衰範囲演算器141は、負荷角度指令θ1＊とパラメータcとを用いて、ｈ＝ｃ｜θ1＊｜である減衰範囲を算出する。
この求められた減衰範囲hは制御切替器144に出力される。
【００７０】
また、減衰パラメータ演算器142は、上記式（20）に従って減衰パラメータγを算出し、場合分け線形トルク演算器143に出力する。
【００７１】
場合分け線形トルク演算器143は、減衰パラメータ演算器142から与えられる減衰パラメータγおよび予め設定された速度比例制御ゲインβ、位置比例制御ゲインκを用い、式（19）の場合分け線形トルクuを算出する。
算出した場合分け線形トルクuは、制御切替器144に出力する。
【００７２】
制御切替器144は、場合分け線形トルク演算器143にて求められた場合分け線形トルクuを負荷角度追従偏差ｅおよび減衰範囲hを参照して切替選択する。
制御切替器144にて選択された場合分け線形トルクはトルク指令演算器111に出力される。
【００７３】
また、非線形トルク演算器153は、式（１５）を用いて算出した車輪垂直加速度推定値と、式（１４）を用いて算出した車輪水平速度推定値に基づいて、式（１２）のＮｘ＋Ｎｙである非線形トルクを算出し出力する。
【００７４】
トルク指令演算器111は、制御切替値ｕと非線形トルクＮｘ＋Ｎｙを用いて式（２１）によりトルク指令Ｔｒｅｆを算出し出力する。
【００７５】
2010267193.tif 000023
【００７６】
ただし、rは車輪半径である。
移動体本体112はトルク指令Trefにより駆動制御される。
【００７７】
このような構成を備える本第1実施形態によれば、次の効果を奏することができる。
（１）本実施形態では、式（１４）の車輪水平速度推定値を用いて移動体本体112を制御するので、車輪202と路面203が相対的にすべる場合にも移動体本体112の車輪水平速度v2（＝x2の一階微分）を車輪水平速度指令v2＊（x2*の一階微分）に一致させることができる。
【００７８】
従来は、単純な線形モデルを想定した制御設計を採用していたため、車輪と路面とがすべるような非線形要素に対して特別な考慮をしていなかった。
そして、従来は、車輪のすべりのような非線形要素も単なる外乱の一つとして押さえこむようにゲイン等を設定していたが、路面との滑りや衝突の影響で車輪が空転し、負荷の姿勢が大きくずれるような場合にはロバスト性を考慮した制御設計であっても安定状態に復帰させるには無理がある。
この点、本実施形態では、式（１４）の車輪水平速度推定値を車輪水平速度推定器152によって推定し、非線形トルク演算器153において、式（12）の車輪水平位置x2をパラメータとする非線形項Nxを求めて、これをトルク指令に加えている。したがって、車輪202と路面203が相対的にすべるような非線形要素にも対応し、移動体本体112の車輪水平速度v2（＝x2の一階微分）を車輪水平速度指令v2＊（x2*の一階微分）に一致させることができる。
【００７９】
（２）本実施形態では、式（１５）の車輪垂直加速度推定値を用いて移動体本体112を制御するので、路面203に凹凸があっても車輪202が路面203に接触している間は負荷角度θ1を安定に制御することができる。
従来は水平な路面を走行する場合を仮定しており、凹凸を乗り越えるような非線形要素を考慮していなかった。そのため、路面凹凸を乗り越える際に負荷が大きく傾くような場合には発振したり、制御不能になって転倒するなどの問題があった。
この点、本実施形態では、式（15）の車輪垂直加速度推定値を車輪垂直加速度推定器151によって算出し、非線形トルク演算器153において、式（12）の車輪垂直位置y2をパラメータとする非線形項Nyを求めて、これをトルク指令に加えている。
したがって、路面凹凸を乗り越えるような非線形要素にも対応し、負荷角度θ1を安定に制御することができる。
【００８０】
（3）本第1実施形態では、式（１９）に示すように、負荷角度θ1を３つの範囲に分け、それぞれの場合に最適なトルク指令を算出する。
そして、制御切替器144において減衰範囲の内と外とで制御を切り替える。
これにより、負荷角度θ1が負荷角度指令θ1＊の近傍において振動することなく、負荷角度指令θ1＊に滑らかに収束するようにすることができる。
その結果、安定した水平走行を実現させることができる。
【００８１】
（4）減衰パラメータ演算器142を備え、式（２０）により減衰パラメータγを設定するので、減衰パラメータγを一定値に固定する場合に比べ、滑らかに速く負荷角度θ1を負荷角度指令θ1＊に収束させることができる。
【００８２】
（実験例）
次に、本発明の効果を実証する実験例を示す。
実験例として、第１実施形態のシミュレーション結果を示す。
ここで、シミュレーションに用いた数値は以下のとおりである。
【００８３】
m1 =70 [kg]、
J1 = 25.2 [kg・m2]、
m2 = 15 [kg]、
J2 = 0.075 [kg・m2]、
l = 0.9 [m]、
r = 0.1 [m]、
D = 0.1 [N・s／m]、
g = 9.8 [m／s2]、
T = 1×10-3 [ｓ]、
κ = 40 [s-1]、
J10 = m1×l2＋J1 [kg・m2]、
β0 = 2πκ [s-1]、
β = β0×J10 [N・m・s／rad]、
γ = 0.1 [N・m・s／rad]、
pcl = [-49.9、-201.4］ [rad／s]、
td = 0.5 [s]
【００８４】
ただし、
m1は負荷質量、
J1は、負荷慣性モーメント、
m2は、車輪質量、
J2は、車輪慣性モーメント、
lは、負荷車輪重心間距離、
rは、車輪半径、
Dは、車輪路面間粘性摩擦、
gは、重力加速度、
Tは、制御周期、
κは、本発明における位置比例制御ゲイン、
J10は、公称慣性モーメント、
β0は、本発明における正規化速度比例制御ゲイン、
βは、本発明における速度比例制御ゲイン、
γは、本発明における摩擦パラメータ、
pclは、従来技術における閉ループ極、
tdは、インパルス外乱時間、である。
【００８５】
なお、
車輪路面間粘性摩擦Dは、図2の車輪202と路面203の間にはたらく粘性摩擦であり、
公称慣性モーメントJ10は、本発明における速度ループを正規化するパラメータであり、
pclは、従来技術における状態フィードバック制御で配置する閉ループの極、である。
また、インパルス外乱時間tdにおいて、車輪202の鉛直上方方向にインパルス状の加速度外乱入力がある場合を考える。
【００８６】
図3、図4はシミュレーション結果を示す図である。
図3は、負荷角度の変化を示す。
図3において、破線L11は負荷角度指令（Load angular position reference input）、実線L10は本発明による負荷角度（Load angular position with proposed control）、一点鎖線L12は従来技術による負荷角度を示す。
加速度外乱を入力する0.5[s]までは、従来技術と本発明とは共に負荷角度指令にほぼ同じ程度に追従しているが、0.5[s]以降は従来技術では負荷角度が発振しているのに対し、本発明では前記加速度外乱入力後も発振することなく前記負荷角度指令に追従していることが分かる。
【００８７】
そして、本発明による負荷角度が折れ線状の時間変化をしているのは、式（１９）の前記減衰範囲において減衰のみを加える制御を実施しているためであり、前記減衰範囲をもたせることにより前記負荷位置が前記負荷位置指令の近傍において振動的になりにくくなっていることが示された。
【００８８】
また、車輪垂直加速度推定器151において、式（１５）の車輪垂直加速度推定値を算出し、非線形トルク演算器153において、非線形トルクNyを算出することにより、路面203の凹凸のため車輪202に加わる垂直方向の前記加速度外乱を補償できるので、本発明は前記加速度外乱が存在する場合にも前記負荷角度を安定化させることが示された。
【００８９】
図4は、車輪の水平速度変化を示す。
図4において、破線L21は所望の車輪水平速度、実線L20は本発明による車輪水平速度、一点鎖線L22は従来技術による車輪水平速度である。
加速度外乱を入力する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…倒立ロボット。
【図１】
2010267193.tif 000029
【図２】
2010267193.tif 000030
【図３】
2010267193.tif 000031
【図４】
2010267193.tif 000032
【図５】
2010267193.tif 000033
【図６】
2010267193.tif 000034
【図７】
2010267193.tif 000035
【図８】
2010267193.tif 000036
【図９】
2010267193.tif 000037