倒立型移動体の制御装置
【課題】倒立型の移動体が振動的になることなく安定に所望の速度で走行できるようにする。
【解決手段】負荷角度θ1が負荷角度指令θ1＊の近傍である場合、減衰のみを移動体本体141に加えるように制御する。すなわち、制御装置130は、場合分け線形トルク演算器105と、制御切替器106と、を備える。場合分け線形トルク演算器105は、負荷速度と減衰パラメータとの乗算値に負号を付した減衰トルクと、位置偏差、速度偏差および加速度偏差の一つ以上に所定ゲインを乗算して得る線形フィードバックトルクと、を算出する。制御切替器106は、前記減衰トルクと前記線形フィードバックトルクとを切り替えて出力する。制御切替器106は、負荷角度θ1が負荷角度指令θ1＊の近傍である場合、減衰トルクを出力し、それ以外の場合には前記線形フィードバックトルクを出力する。
【解決手段】負荷角度θ1が負荷角度指令θ1＊の近傍である場合、減衰のみを移動体本体141に加えるように制御する。すなわち、制御装置130は、場合分け線形トルク演算器105と、制御切替器106と、を備える。場合分け線形トルク演算器105は、負荷速度と減衰パラメータとの乗算値に負号を付した減衰トルクと、位置偏差、速度偏差および加速度偏差の一つ以上に所定ゲインを乗算して得る線形フィードバックトルクと、を算出する。制御切替器106は、前記減衰トルクと前記線形フィードバックトルクとを切り替えて出力する。制御切替器106は、負荷角度θ1が負荷角度指令θ1＊の近傍である場合、減衰トルクを出力し、それ以外の場合には前記線形フィードバックトルクを出力する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、倒立型移動体の制御装置に関する。詳しくは、車輪駆動手段とリンク状の負荷体とを備え、リンク状の負荷体を倒立させるバランス制御を行いながら移動する倒立型移動体の制御装置に関する。
【背景技術】
【０００２】
同軸に設けられた左右一対の車輪を有し、倒立状態を維持しながら走行する移動装置が知られている。たとえば、特許文献1（特開2006−123014号公報）には、倒立状態を維持しながら自律的に走行する倒立二輪走行ロボットが開示されている。また、特許文献2（特開2006-315666号公報）には、人が立位姿勢でステップに搭乗した状態でバランスを保ちながら走行する同軸二輪車が開示されている。
【０００３】
図10は、特許文献1に開示された倒立二輪走行ロボットの制御器の構成を示す図である。
図10において、1001は摩擦推定器、1002は目標状態生成器、1003は状態フィードバックゲイン、1004は倒立ロボットである。
摩擦推定器1001には角速度指令値が入力され、摩擦推定器1001は、モータの摩擦および車輪と路面との摩擦を摩擦推定値として算出し出力する。
目標状態生成器1002には前記角速度指令値と前記摩擦推定値とが入力され、目標状態生成器1002は、制御対象である倒立ロボット1004の目標状態を算出し出力する。
状態フィードバックゲイン1003には、前記目標状態から倒立ロボット1004の状態変数を減算した信号が入力され、状態フィードバックゲイン1003は、その入力信号に基づいて倒立ロボット1004に所望の動作をさせる状態フィードバック信号を算出して出力する。
倒立ロボット1004は、前記摩擦推定値と前記状態フィードバック信号との加算値により駆動される。
このように、従来技術の倒立二輪走行ロボットの制御では、制御対象である倒立ロボット1004を所望の姿勢の近傍で線形化した線形化モデルに基づいて倒立ロボット1004の動作を制御している。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開2006-123014号公報（図4）
【発明の概要】
【発明が解決しようとする課題】
【０００５】
上記に説明したように、従来の倒立制御では、単純な線形フィードバック制御を行っているが、倒立ロボットや同軸二輪車では、同軸車輪の上に長いリンクが設けられた構造を有しているので、負荷であるリンク部分が目標姿勢で安定せずに揺れやすいという特徴がある。
このような構成に対して、従来のように目標姿勢からの偏差に応じたフィードバックをかけ続けると、目標姿勢で安定せずに目標の近傍で振動が起きてしまうという問題があった。
なお、従来は、このような目標近傍での振動に対し、振動を抑えるようにフィードバックゲインを調整していたが、目標から大きく離れた場合には倒れてしまわないように素早く目標姿勢に復帰させる一方で、目標近傍では発振を防ぐという制御を単純なフィードバックゲインの調整だけで行うには無理があった。
【０００６】
本発明の目的は、発振させることなく安定した倒立バランス制御を行いつつ所望の水平速度で走行させることができる倒立型移動体の制御装置を提供することにある。
【課題を解決するための手段】
【０００７】
上記問題を解決するため、本発明は、次のように構成したのである。
すなわち、本発明の倒立型移動体の制御装置は、車輪を有する駆動手段とリンクを介して前記車輪の上で倒立制御される負荷とを有する移動体本体を倒立させながら走行制御をする制御装置であって、前記負荷の重心と前記車輪の重心とをつなぐ直線が鉛直線との間になす角を負荷角度とし、前記負荷角度が所望の前記負荷角度である負荷角度指令の近傍である場合、減衰のみを前記移動体本体に加えるように制御することを特徴とする。
【０００８】
本発明では、前記負荷角度指令の近傍の幅としての減衰範囲は、前記負荷角度指令の絶対値に所定の係数を乗算して算出することが好ましい。
【０００９】
本発明では、前記減衰を粘性摩擦とすることが好ましい。
【００１０】
本発明では、前記負荷角度指令から前記負荷角度を減算した値である負荷角度追従偏差と、前記負荷角度指令と、の関数として前記粘性摩擦である減衰パラメータを算出することが好ましい。
【００１１】
本発明では、前記負荷角度指令の絶対値を２で除算した値を前記負荷角度追従偏差から減算し、その絶対値を前記負荷角度指令の絶対値で除算し、さらに定数を乗算することによって前記減衰パラメータを算出することが好ましい。
【００１２】
本発明では、前記粘性摩擦である減衰パラメータを一定値とすることが好ましい。
【００１３】
本発明では、前記負荷速度と前記減衰パラメータとの乗算値に負号を付した減衰トルクと、位置偏差、速度偏差および加速度偏差の一つ以上に所定ゲインを乗算して得る線形フィードバックトルクと、を算出する場合分け線形トルク演算器と、前記場合分け線形トルク演算器で算出された前記減衰トルクと前記線形フィードバックトルクとを切り替えて出力する制御切替器と、を備えることが好ましい。
【００１４】
本発明では、
前記制御切替器は、
0 ≦ sgn（θ1＊）・e< h
の場合には、前記減衰トルクを出力し、
それ以外の場合には前記線形フィードバックトルクを出力する
ことが好ましい。
ただし、ｅ＝θ1＊−θ1であり、θ1＊は負荷角度指令であり、θ1は負荷角度であり、ｓｇｎ（・）は、・が正数のとき＋１、負数のとき−１、零のとき０の値をとるシグナム関数であり、hは前記負荷角度指令の絶対値に所定の係数を乗算して算出する減衰範囲である。
【発明の効果】
【００１５】
このような本発明によれば、移動体の負荷角度が所望の値の近傍で振動しないようにすることができる。そして、移動体の負荷角度を振動させることなく所望の値に速く収束させ、移動体を所望の速度で安全に走行させることができる。
【図面の簡単な説明】
【００１６】
【図１】本発明の倒立型移動体に係る第１実施形態を示す図。
【図２】移動体本体をモデル化した図。
【図３】シミュレーション結果を示す図。
【図４】シミュレーション結果を示す図。
【図５】変形例1を示す図。
【図６】変形例2を示す図。
【図７】倒立型移動体として同軸二輪車を示す図。
【図８】倒立型移動体として倒立型自律走行ロボットを示す図。
【図９】4輪の車輪駆動手段の上に揺動可能にリンク機構を備えた倒立型移動体を示す図。
【図１０】従来の倒立二輪走行ロボットの制御器の構成を示す図。
【発明を実施するための最良の形態】
【００１７】
以下、本発明の実施の形態について図を参照して説明する。
（第1実施形態）
【００１８】
図1は、本発明の倒立型移動体に係る第１実施形態を示す図である。
倒立型移動体は、制御対象としての移動体本体141と、移動体本体141の状態を検出する状態センサ142と、所望の目標状態を指令する指令部100と、状態センサ142による検出結果と指令部100からの指令値とに基づいて移動体本体141の制御を実行する制御装置110と、を備える。
【００１９】
移動体本体141としては、同軸二輪車（図7）、倒立型自律走行ロボット（図8）などが一般的な例として挙げられる。
これに限らず、車輪による駆動手段とリンク状の負荷体とを備え、リンク状の負荷体を倒立させるバランス制御を行うものであればよい。
たとえば、図9に示すような構成でもよい。
図9は、4輪の車輪駆動手段901の上に揺動可能にリンク機構902が設けられている構成である。
たとえば、リンク機構902の上部をカゴ状903にして、物品を載せて運ぶようにしてもよい。
【００２０】
そして、以下の説明では、このような移動体本体141を、図2のようにモデル化する。
ここで、図2において、201は負荷、202は車輪、203は路面である。
図2に示すように移動体本体141は倒立して走行するものとする。
負荷201は、ロボットのボディや、移動体141に乗る人または荷物である。
車輪202は、負荷201を乗せ、回転することにより路面203との摩擦力を利用して移動する。
【００２１】
状態センサ142は、負荷201の角度（θ1）と、車輪202の角度（θ2）を検出するものである。
【００２２】
指令部100は、車輪水平速度指令生成器101と、負荷角度指令演算器102と、を備える。
車輪水平速度指令生成器101は、移動体本体141の車輪202の所望の水平移動速度である車輪水平速度指令を生成し出力する。
負荷角度指令演算器102は、車輪水平速度指令を入力とし、移動体本体141が走行する路面203が水平な場合に車輪水平速度が車輪水平速度指令に追従するような負荷角度である負荷角度指令を算出し出力する。
【００２３】
制御装置110は、場合分け線形制御部120と、非線形制御部130と、トルク指令演算器111と、を備える。
【００２４】
場合分け線形制御部120は、減衰範囲演算器121と、減衰パラメータ演算器122と、場合分け線形トルク演算器123と、制御切替器124と、を備える。
【００２５】
減衰範囲演算器121には、負荷角度指令演算器102からの負荷角度指令と、状態センサ142による検出値である負荷角度θ1および車輪角度θ2と、が入力される。
減衰範囲演算器121は、入力信号に基づき、移動体本体141の動作制御において粘性摩擦を用いた減衰のみ加える負荷角度の範囲を減衰範囲として算出し出力する。
【００２６】
減衰パラメータ演算器122には、負荷角度指令演算器102からの負荷角度指令（θ1＊）と、状態センサ142による検出値である負荷角度（θ1）および車輪角度（θ2）と、が入力される。
減衰パラメータ演算器122は、入力信号に基づき、前記減衰範囲における制御に用いる減衰パラメータを算出し出力する。
【００２７】
場合分け線形トルク演算器123には、負荷角度指令演算器102からの負荷角度指令（θ1＊）と、減衰パラメータ演算器122からの減衰パラメータと、状態センサ142による検出値である負荷角度（θ1）および車輪角度（θ2）と、が入力される。
場合分け線形トルク演算器123は、負荷速度と減衰パラメータとの乗算値に負号を付した減衰トルクと、位置偏差、速度偏差および加速度偏差の一つ以上に所定ゲインを乗算して得る線形フィードバックトルクと、を算出して出力する。
【００２８】
制御切替器124には、減衰範囲演算器121にて算出された減衰範囲と、状態センサ142による検出値と、場合分け線形トルク演算器123にて算出された場合分け線形トルクと、が入力されている。
制御切替器124は、場合分け線形トルク演算器123にて算出された場合分け線形トルクを切り替えて出力する。
【００２９】
非線形制御部130は、車輪垂直加速度推定器131と、車輪水平速度推定器132と、非線形トルク演算器133と、を備える。
車輪垂直加速度推定器131には、状態センサ142からの検出信号が入力され、車輪垂直加速度推定器131は、その入力信号に基づいて車輪202の垂直加速度を推定し車輪垂直加速度推定値として出力する。
車輪水平速度推定器132には、状態センサ142からの検出信号が入力され、車輪水平速度推定器132は、その入力信号に基づいて車輪202の水平速度を推定し車輪水平速度推定値として出力する。
非線形トルク演算器133には、前記車輪垂直加速度推定値と前記車輪水平速度推定値とが入力され、非線形トルク演算器133は、移動体本体141の非線形ダイナミクスを示す非線形トルクを演算し出力する。
【００３０】
トルク指令演算器111には、制御切替器124で切り替え出力される前記場合分け線形トルクと非線形トルク演算器133から出力される前記非線形トルクとが入力され、トルク指令演算器111は、これら入力信号の加算値を前記車輪202の半径で除算して得たトルク指令を出力する。
【００３１】
移動体本体141は前記トルク指令により駆動される。
【００３２】
以下、本第1実施形態に係る制御装置110が移動体本体141の動作を制御する仕組みの詳細を説明する。
【００３３】
図2において、次のようにパラメータを設定する。
ｍ１は負荷質量、
Ｊ１は負荷慣性モーメント、
ｍ２は車輪質量、
Ｊ２は車輪慣性モーメント、
ｌは負荷と車輪の重心間距離である負荷車輪重心間距離、
ｒは車輪半径、
θ１は負荷角度、
θ２は車輪角度、
Ｔｒｅｆはトルク指令、とする。
さらに、車輪水平位置をｘ２、車輪垂直位置をｙ２とすると、負荷水平位置ｘ１および負荷垂直位置ｙ１は次のようにそれぞれ式（１）と式（２）とであらわされる。
【００３４】
【００３５】
【００３６】
式（１）と式（２）とを用い、移動体本体141の運動エネルギーＴおよびポテンシャルエネルギーＶは次のようにそれぞれ式（３）と式（４）とであらわされる。
【００３７】
【００３８】
【００３９】
すると、オイラーラグランジュ方程式を用い、移動体本体141の運動方程式は式（５）乃至（８）と求められる。
【００４０】
【００４１】
【００４２】
【００４３】
【００４４】
ただし、ｇは重力加速度である。
さらに、車輪202と路面203の間の粘性摩擦を考慮すると、式（６）と式（７）は式（９）と式（１０）と書き換えられる。
ここに、Dは粘性摩擦係数とする。
【００４５】
【００４６】
【００４７】
式（９）と式（１０）とを用い、式（１１）が得られる。
【００４８】
【００４９】
式（５）から式（１１）を減算すると式（１２）が得られる。
【００５０】
【００５１】
ただし、Nxは車輪水平位置x2をパラメータとした非線形項、Nyは車輪垂直位置y2をパラメータとした非線形項である。
【００５２】
負荷角度θ1は車輪水平位置x2より十分に遅く変化すると仮定すると、式（１１）は式（１３）と書き換えられる。
【００５３】
【００５４】
ただし、車輪水平位置x2より十分に遅く変化する部分を定数ｃ１、ｃ２、ｃ３とした。
式（１３）より、車輪水平速度dx2／dtは式（１４）であらわされる。
【００５５】
【００５６】
ただし、ｓ2・Θ2は車輪加速度（d2θ2／dt2）のラプラス変換、Ｌはラプラス演算子である。
車輪水平速度推定器132は式（１４）を用いて前記車輪水平速度推定値を算出する。
【００５７】
一方、式（８）を車輪垂直加速度（d2y2／dt2）について解くと式（１５）が得られる。
【００５８】
【００５９】
車輪垂直加速度推定器131は、式（１５）を用いて前記車輪垂直加速度推定値を算出する。
【００６０】
所望の車輪水平速度である車輪水平速度指令をv2＊（＝x2*の一階微分）とすると、路面203が水平で平らな場合に車輪水平速度dx2／dtが前記車輪水平速度指令v2*となる負荷角度θ1である負荷角度指令θ1＊は式（１６）であらわされる。すなわち、負荷角度指令は、車輪水平加速度指令を重力加速度で除算した値の逆正接である。
【００６１】
【００６２】
車輪水平速度指令生成器101は、車輪水平速度指令v2＊（＝x2*の一階微分）を出力し、負荷角度指令演算器102は、式（１６）を用いて負荷角度指令θ1＊を算出し出力する。
【００６３】
式（１４）と式（１５）を式（１２）に代入すると、式（１７）が得られる。
【００６４】
【００６５】
式（１７）を書き換えると式（１８）とあらわされる。
【００６６】
【００６７】
ただし、ｕは場合分け線形トルクである。
そして、負荷角度θ1が負荷角度指令θ1＊に収束するような、式（１９）に示す場合分け線形トルクｕを考える。
【００６８】
【００６９】
ただし、ｅ＝θ1＊−θ1は、負荷角度追従偏差、
βは、速度比例制御ゲイン、
κは、位置比例制御ゲイン、
γは、減衰パラメータ、
ｓｇｎ（・）は、・が正数のとき＋１、負数のとき−１、零のとき０の値をとるシグナム関数である。
【００７０】
また、ｈ＝ｃ｜θ1＊｜は、フィードバック制御によるチャタリングを抑制するために設けた負荷角度範囲である減衰範囲であり、ｃは、減衰範囲ｈのパラメータである。
【００７１】
ここで、式（１９）に表される場合分け線形トルクｕの意味は、負荷角度θ1と負荷角度指令θ1＊との偏差が小さい場合、具体的には、負荷角度θ1の絶対値が減少する方向において負荷角度θ1と負荷角度指令θ1＊との角度ずれが減衰範囲ｈの範囲内である場合、負荷201には粘性摩擦が減衰パラメータγである動作をさせることにより、負荷角度θ1を負荷角度指令θ1＊に安定して収束させることができることを表す。
また、負荷角度θ1と負荷角度指令θ1＊とのずれが上記以外の場合、負荷201には、剛性が位置比例制御ゲインκで、粘性摩擦が速度比例制御ゲインβであるフィードバック制御をかけることにより、負荷角度θ1を負荷角度指令θ1＊に収束させることができることを表す。
【００７２】
ここで、減衰パラメータγを例えば式（２０）に示すように負荷角度指令θ1＊と負荷角度追従偏差ｅの関数として設定すると好適である。
【００７３】
【００７４】
すなわち、場合分け線形制御部120が負荷角度指令演算器102から負荷角度指令θ1＊を受けると、負荷角度指令θ1＊は減衰範囲演算器121、減衰パラメータ演算器122および場合分け線形トルク演算器123に入力される。
そして、減衰範囲演算器121は、負荷角度指令θ1＊とパラメータcとを用いて、ｈ＝ｃ｜θ1＊｜である減衰範囲を算出する。
この求められた減衰範囲hは制御切替器124に出力される。
【００７５】
また、減衰パラメータ演算器122は、上記式（20）に従って減衰パラメータγを算出し、場合分け線形トルク演算器123に出力する。
【００７６】
場合分け線形トルク演算器123は、減衰パラメータ演算器122から与えられる減衰パラメータγおよび予め設定された速度比例制御ゲインβ、位置比例制御ゲインκを用い、式（19）の場合分け線形トルクuを算出する。
算出した場合分け線形トルクuは、制御切替器124に出力する。
【００７７】
制御切替器124は、場合分け線形トルク演算器123にて求められた場合分け線形トルクuを負荷角度追従偏差ｅおよび減衰範囲hを参照して切替選択する。
制御切替器124にて選択された場合分け線形トルクはトルク指令演算器111に出力される。
【００７８】
また、非線形トルク演算器133は、式（１５）を用いて算出した車輪垂直加速度推定値と、式（１４）を用いて算出した車輪水平速度推定値に基づいて、式（１２）のＮｘ＋Ｎｙである非線形トルクを算出し出力する。
【００７９】
トルク指令演算器111は、場合分け線形トルクｕと非線形トルクＮｘ＋Ｎｙを用いて式（２１）によりトルク指令Ｔｒｅｆを算出し出力する。
【００８０】
【００８１】
ただし、rは車輪半径である。
移動体本体141はトルク指令Ｔｒｅｆにより駆動制御される。
【００８２】
このような構成を備える本第1実施形態によれば、次の効果を奏することができる。
（１）本第1実施形態では、式（１９）に示すように、負荷角度θ1を３つの範囲に分け、それぞれの場合に最適なトルク指令を算出する。
そして、制御切替器124において減衰範囲の内と外とで制御を切り替える。
これにより、負荷角度θ1が負荷角度指令θ1＊の近傍において振動することなく、負荷角度指令θ1＊に滑らかに収束するようにすることができる。
その結果、安定した水平走行を実現させることができる。
【００８３】
（２）減衰パラメータ演算器122を備え、式（２０）により減衰パラメータγを設定するので、減衰パラメータγを一定値に固定する場合に比べ、滑らかに速く負荷角度θ1を負荷角度指令θ1＊に収束させることができる。
【００８４】
（３） 式（１４）の車輪水平速度推定値を用いて移動体本体141を制御するので、車輪202と路面203が相対的にすべる場合にも移動体本体141の車輪水平速度v2（＝x2の一階微分）を車輪水平速度指令v2＊（x2*の一階微分）に一致させることができる。
【００８５】
（４） 式（１５）の車輪垂直加速度推定値を用いて移動体本体141を制御するので、路面203に凹凸があっても車輪202が路面203に接触している間は負荷角度θ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]以降は従来技術では負荷角度が発振しているのに対し、本発明では前記加速度外乱入力後も発振することなく前記負荷角度指令に追従していることが分かる。
【００９１】
そして、本発明による負荷角度が折れ線状の時間変化をしているのは、式（１９）の前記減衰範囲において減衰のみを加える制御を実施しているためであり、前記減衰範囲をもたせることにより前記負荷位置が前記負荷位置指令の近傍において振動的になりにくくなっていることが示された。
【００９２】
また、車輪垂直加速度推定器131において、式（１５）の車輪垂直加速度推定値を算出し、非線形トルク演算器133において、非線形トルクNyを算出することにより、路面203の凹凸のため車輪202に加わる垂直方向の前記加速度外乱を補償できるので、本発明は前記加速度外乱が存在する場合にも前記負荷角度を安定化させることが示された。
【００９３】
図4は、車輪の水平速度変化を示す。
図4において、破線L21は所望の車輪水平速度、実線L20は本発明による車輪水平速度、一点鎖線L22は従来技術による車輪水平速度である。
加速度外乱を入力する0.5[s]までは従来技術も本発明も共に前記所望の車輪水平速度に追従しているが、0.5[s]以降において従来技術は振動的となるのに対し、本発明は振動的とならず前記所望の車輪水平速度に追従していることがわかる。
【００９４】
（変形例1）
上記第1実施形態においては、減衰パラメータγは、上記式（20）に従って減衰パラメータ演算器122により算出したものを用いたが、減衰パラメータγは予め設定された固定値としてもよい。
すなわち、図5に示すように、予め設定された減衰パラメータを減衰パラメータ記憶部125に設定記憶させておき、減衰パラメータ記憶部125から場合分け線形トルク演算器123に減衰パラメータγの値を供給するようにしてもよい。
【００９５】
（変形例2）
また、上記第1実施形態では、最も好適な実施形態として非線形制御部130を有する場合を説明したが、負荷角度指令θ1＊の近傍において振動することなく負荷角度θ1を負荷角度指令θ1＊に滑らかに収束させる安定した制御を実現させる場合、非線形制御部130は無くてもよい。
すなわち、図6に示すように、非線形制御部130を省略した制御装置610であってもよい。
【００９６】
ここで、非線形制御部130を省略して場合分け線形制御部120だけを備える場合には、場合分け線形トルク演算器123にて算出する場合分け線形トルクｕの線形フィードバックトルクのゲインを調整して、外乱となる非線形要素をできる限り抑え込むようなゲインを設定することが好まししい。
【００９７】
ただし、水平ではない凹凸路面の走行や障害物との衝突、車輪の空転などの非線形要素に対応するためには非線形制御部130を合わせもっていた方がよいことはもちろんである。
【００９８】
なお、本発明は上記実施形態に限定されず、本発明の目的を達成できる範囲での変形、改良等は本発明に含まれる。
例えば、式（１９）において、位置Ｐ／速度Ｐ制御は、位置Ｐ／速度ＰＩ制御、位置Ｐ／速度Ｉ−Ｐ制御、位置ＰＩＤ制御など、任意の制御則に置き換えても良いことはもちろんである。
【産業上の利用可能性】
【００９９】
本発明は、路面に凹凸がある場合にも、人や物体と衝突した場合にも、転倒することなく、発振することなく２輪倒立の移動体を所望の水平速度で走行させることができるので、２輪倒立で走行するロボット、電動車椅子、自動搬送装置、災害時の人命救助など狭い場所で作業するロボット、振動に弱い電子部品の組み立て装置などに広く適用できる。
【符号の説明】
【０１００】
100…指令部、101…車輪水平速度指令生成器、102…負荷角度指令演算器、111…トルク指令演算器、120…場合分け線形制御部、121…減衰範囲演算器、122…減衰パラメータ演算器、123…場合分け線形トルク演算器、124…制御切替器、130…非線形制御部、131…車輪垂直加速度推定器、132…車輪水平速度推定器、133…非線形トルク演算器、141…移動体本体、142…状態センサ、125…減衰パラメータ記憶部、110、610…制御装置、201…負荷、202…車輪、203…路面、1001…摩擦推定器、1002…目標状態生成器、1003…状態フィードバックゲイン、1004…倒立ロボット。
【技術分野】
【０００１】
本発明は、倒立型移動体の制御装置に関する。詳しくは、車輪駆動手段とリンク状の負荷体とを備え、リンク状の負荷体を倒立させるバランス制御を行いながら移動する倒立型移動体の制御装置に関する。
【背景技術】
【０００２】
同軸に設けられた左右一対の車輪を有し、倒立状態を維持しながら走行する移動装置が知られている。たとえば、特許文献1（特開2006−123014号公報）には、倒立状態を維持しながら自律的に走行する倒立二輪走行ロボットが開示されている。また、特許文献2（特開2006-315666号公報）には、人が立位姿勢でステップに搭乗した状態でバランスを保ちながら走行する同軸二輪車が開示されている。
【０００３】
図10は、特許文献1に開示された倒立二輪走行ロボットの制御器の構成を示す図である。
図10において、1001は摩擦推定器、1002は目標状態生成器、1003は状態フィードバックゲイン、1004は倒立ロボットである。
摩擦推定器1001には角速度指令値が入力され、摩擦推定器1001は、モータの摩擦および車輪と路面との摩擦を摩擦推定値として算出し出力する。
目標状態生成器1002には前記角速度指令値と前記摩擦推定値とが入力され、目標状態生成器1002は、制御対象である倒立ロボット1004の目標状態を算出し出力する。
状態フィードバックゲイン1003には、前記目標状態から倒立ロボット1004の状態変数を減算した信号が入力され、状態フィードバックゲイン1003は、その入力信号に基づいて倒立ロボット1004に所望の動作をさせる状態フィードバック信号を算出して出力する。
倒立ロボット1004は、前記摩擦推定値と前記状態フィードバック信号との加算値により駆動される。
このように、従来技術の倒立二輪走行ロボットの制御では、制御対象である倒立ロボット1004を所望の姿勢の近傍で線形化した線形化モデルに基づいて倒立ロボット1004の動作を制御している。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開2006-123014号公報（図4）
【発明の概要】
【発明が解決しようとする課題】
【０００５】
上記に説明したように、従来の倒立制御では、単純な線形フィードバック制御を行っているが、倒立ロボットや同軸二輪車では、同軸車輪の上に長いリンクが設けられた構造を有しているので、負荷であるリンク部分が目標姿勢で安定せずに揺れやすいという特徴がある。
このような構成に対して、従来のように目標姿勢からの偏差に応じたフィードバックをかけ続けると、目標姿勢で安定せずに目標の近傍で振動が起きてしまうという問題があった。
なお、従来は、このような目標近傍での振動に対し、振動を抑えるようにフィードバックゲインを調整していたが、目標から大きく離れた場合には倒れてしまわないように素早く目標姿勢に復帰させる一方で、目標近傍では発振を防ぐという制御を単純なフィードバックゲインの調整だけで行うには無理があった。
【０００６】
本発明の目的は、発振させることなく安定した倒立バランス制御を行いつつ所望の水平速度で走行させることができる倒立型移動体の制御装置を提供することにある。
【課題を解決するための手段】
【０００７】
上記問題を解決するため、本発明は、次のように構成したのである。
すなわち、本発明の倒立型移動体の制御装置は、車輪を有する駆動手段とリンクを介して前記車輪の上で倒立制御される負荷とを有する移動体本体を倒立させながら走行制御をする制御装置であって、前記負荷の重心と前記車輪の重心とをつなぐ直線が鉛直線との間になす角を負荷角度とし、前記負荷角度が所望の前記負荷角度である負荷角度指令の近傍である場合、減衰のみを前記移動体本体に加えるように制御することを特徴とする。
【０００８】
本発明では、前記負荷角度指令の近傍の幅としての減衰範囲は、前記負荷角度指令の絶対値に所定の係数を乗算して算出することが好ましい。
【０００９】
本発明では、前記減衰を粘性摩擦とすることが好ましい。
【００１０】
本発明では、前記負荷角度指令から前記負荷角度を減算した値である負荷角度追従偏差と、前記負荷角度指令と、の関数として前記粘性摩擦である減衰パラメータを算出することが好ましい。
【００１１】
本発明では、前記負荷角度指令の絶対値を２で除算した値を前記負荷角度追従偏差から減算し、その絶対値を前記負荷角度指令の絶対値で除算し、さらに定数を乗算することによって前記減衰パラメータを算出することが好ましい。
【００１２】
本発明では、前記粘性摩擦である減衰パラメータを一定値とすることが好ましい。
【００１３】
本発明では、前記負荷速度と前記減衰パラメータとの乗算値に負号を付した減衰トルクと、位置偏差、速度偏差および加速度偏差の一つ以上に所定ゲインを乗算して得る線形フィードバックトルクと、を算出する場合分け線形トルク演算器と、前記場合分け線形トルク演算器で算出された前記減衰トルクと前記線形フィードバックトルクとを切り替えて出力する制御切替器と、を備えることが好ましい。
【００１４】
本発明では、
前記制御切替器は、
0 ≦ sgn（θ1＊）・e< h
の場合には、前記減衰トルクを出力し、
それ以外の場合には前記線形フィードバックトルクを出力する
ことが好ましい。
ただし、ｅ＝θ1＊−θ1であり、θ1＊は負荷角度指令であり、θ1は負荷角度であり、ｓｇｎ（・）は、・が正数のとき＋１、負数のとき−１、零のとき０の値をとるシグナム関数であり、hは前記負荷角度指令の絶対値に所定の係数を乗算して算出する減衰範囲である。
【発明の効果】
【００１５】
このような本発明によれば、移動体の負荷角度が所望の値の近傍で振動しないようにすることができる。そして、移動体の負荷角度を振動させることなく所望の値に速く収束させ、移動体を所望の速度で安全に走行させることができる。
【図面の簡単な説明】
【００１６】
【図１】本発明の倒立型移動体に係る第１実施形態を示す図。
【図２】移動体本体をモデル化した図。
【図３】シミュレーション結果を示す図。
【図４】シミュレーション結果を示す図。
【図５】変形例1を示す図。
【図６】変形例2を示す図。
【図７】倒立型移動体として同軸二輪車を示す図。
【図８】倒立型移動体として倒立型自律走行ロボットを示す図。
【図９】4輪の車輪駆動手段の上に揺動可能にリンク機構を備えた倒立型移動体を示す図。
【図１０】従来の倒立二輪走行ロボットの制御器の構成を示す図。
【発明を実施するための最良の形態】
【００１７】
以下、本発明の実施の形態について図を参照して説明する。
（第1実施形態）
【００１８】
図1は、本発明の倒立型移動体に係る第１実施形態を示す図である。
倒立型移動体は、制御対象としての移動体本体141と、移動体本体141の状態を検出する状態センサ142と、所望の目標状態を指令する指令部100と、状態センサ142による検出結果と指令部100からの指令値とに基づいて移動体本体141の制御を実行する制御装置110と、を備える。
【００１９】
移動体本体141としては、同軸二輪車（図7）、倒立型自律走行ロボット（図8）などが一般的な例として挙げられる。
これに限らず、車輪による駆動手段とリンク状の負荷体とを備え、リンク状の負荷体を倒立させるバランス制御を行うものであればよい。
たとえば、図9に示すような構成でもよい。
図9は、4輪の車輪駆動手段901の上に揺動可能にリンク機構902が設けられている構成である。
たとえば、リンク機構902の上部をカゴ状903にして、物品を載せて運ぶようにしてもよい。
【００２０】
そして、以下の説明では、このような移動体本体141を、図2のようにモデル化する。
ここで、図2において、201は負荷、202は車輪、203は路面である。
図2に示すように移動体本体141は倒立して走行するものとする。
負荷201は、ロボットのボディや、移動体141に乗る人または荷物である。
車輪202は、負荷201を乗せ、回転することにより路面203との摩擦力を利用して移動する。
【００２１】
状態センサ142は、負荷201の角度（θ1）と、車輪202の角度（θ2）を検出するものである。
【００２２】
指令部100は、車輪水平速度指令生成器101と、負荷角度指令演算器102と、を備える。
車輪水平速度指令生成器101は、移動体本体141の車輪202の所望の水平移動速度である車輪水平速度指令を生成し出力する。
負荷角度指令演算器102は、車輪水平速度指令を入力とし、移動体本体141が走行する路面203が水平な場合に車輪水平速度が車輪水平速度指令に追従するような負荷角度である負荷角度指令を算出し出力する。
【００２３】
制御装置110は、場合分け線形制御部120と、非線形制御部130と、トルク指令演算器111と、を備える。
【００２４】
場合分け線形制御部120は、減衰範囲演算器121と、減衰パラメータ演算器122と、場合分け線形トルク演算器123と、制御切替器124と、を備える。
【００２５】
減衰範囲演算器121には、負荷角度指令演算器102からの負荷角度指令と、状態センサ142による検出値である負荷角度θ1および車輪角度θ2と、が入力される。
減衰範囲演算器121は、入力信号に基づき、移動体本体141の動作制御において粘性摩擦を用いた減衰のみ加える負荷角度の範囲を減衰範囲として算出し出力する。
【００２６】
減衰パラメータ演算器122には、負荷角度指令演算器102からの負荷角度指令（θ1＊）と、状態センサ142による検出値である負荷角度（θ1）および車輪角度（θ2）と、が入力される。
減衰パラメータ演算器122は、入力信号に基づき、前記減衰範囲における制御に用いる減衰パラメータを算出し出力する。
【００２７】
場合分け線形トルク演算器123には、負荷角度指令演算器102からの負荷角度指令（θ1＊）と、減衰パラメータ演算器122からの減衰パラメータと、状態センサ142による検出値である負荷角度（θ1）および車輪角度（θ2）と、が入力される。
場合分け線形トルク演算器123は、負荷速度と減衰パラメータとの乗算値に負号を付した減衰トルクと、位置偏差、速度偏差および加速度偏差の一つ以上に所定ゲインを乗算して得る線形フィードバックトルクと、を算出して出力する。
【００２８】
制御切替器124には、減衰範囲演算器121にて算出された減衰範囲と、状態センサ142による検出値と、場合分け線形トルク演算器123にて算出された場合分け線形トルクと、が入力されている。
制御切替器124は、場合分け線形トルク演算器123にて算出された場合分け線形トルクを切り替えて出力する。
【００２９】
非線形制御部130は、車輪垂直加速度推定器131と、車輪水平速度推定器132と、非線形トルク演算器133と、を備える。
車輪垂直加速度推定器131には、状態センサ142からの検出信号が入力され、車輪垂直加速度推定器131は、その入力信号に基づいて車輪202の垂直加速度を推定し車輪垂直加速度推定値として出力する。
車輪水平速度推定器132には、状態センサ142からの検出信号が入力され、車輪水平速度推定器132は、その入力信号に基づいて車輪202の水平速度を推定し車輪水平速度推定値として出力する。
非線形トルク演算器133には、前記車輪垂直加速度推定値と前記車輪水平速度推定値とが入力され、非線形トルク演算器133は、移動体本体141の非線形ダイナミクスを示す非線形トルクを演算し出力する。
【００３０】
トルク指令演算器111には、制御切替器124で切り替え出力される前記場合分け線形トルクと非線形トルク演算器133から出力される前記非線形トルクとが入力され、トルク指令演算器111は、これら入力信号の加算値を前記車輪202の半径で除算して得たトルク指令を出力する。
【００３１】
移動体本体141は前記トルク指令により駆動される。
【００３２】
以下、本第1実施形態に係る制御装置110が移動体本体141の動作を制御する仕組みの詳細を説明する。
【００３３】
図2において、次のようにパラメータを設定する。
ｍ１は負荷質量、
Ｊ１は負荷慣性モーメント、
ｍ２は車輪質量、
Ｊ２は車輪慣性モーメント、
ｌは負荷と車輪の重心間距離である負荷車輪重心間距離、
ｒは車輪半径、
θ１は負荷角度、
θ２は車輪角度、
Ｔｒｅｆはトルク指令、とする。
さらに、車輪水平位置をｘ２、車輪垂直位置をｙ２とすると、負荷水平位置ｘ１および負荷垂直位置ｙ１は次のようにそれぞれ式（１）と式（２）とであらわされる。
【００３４】
【００３５】
【００３６】
式（１）と式（２）とを用い、移動体本体141の運動エネルギーＴおよびポテンシャルエネルギーＶは次のようにそれぞれ式（３）と式（４）とであらわされる。
【００３７】
【００３８】
【００３９】
すると、オイラーラグランジュ方程式を用い、移動体本体141の運動方程式は式（５）乃至（８）と求められる。
【００４０】
【００４１】
【００４２】
【００４３】
【００４４】
ただし、ｇは重力加速度である。
さらに、車輪202と路面203の間の粘性摩擦を考慮すると、式（６）と式（７）は式（９）と式（１０）と書き換えられる。
ここに、Dは粘性摩擦係数とする。
【００４５】
【００４６】
【００４７】
式（９）と式（１０）とを用い、式（１１）が得られる。
【００４８】
【００４９】
式（５）から式（１１）を減算すると式（１２）が得られる。
【００５０】
【００５１】
ただし、Nxは車輪水平位置x2をパラメータとした非線形項、Nyは車輪垂直位置y2をパラメータとした非線形項である。
【００５２】
負荷角度θ1は車輪水平位置x2より十分に遅く変化すると仮定すると、式（１１）は式（１３）と書き換えられる。
【００５３】
【００５４】
ただし、車輪水平位置x2より十分に遅く変化する部分を定数ｃ１、ｃ２、ｃ３とした。
式（１３）より、車輪水平速度dx2／dtは式（１４）であらわされる。
【００５５】
【００５６】
ただし、ｓ2・Θ2は車輪加速度（d2θ2／dt2）のラプラス変換、Ｌはラプラス演算子である。
車輪水平速度推定器132は式（１４）を用いて前記車輪水平速度推定値を算出する。
【００５７】
一方、式（８）を車輪垂直加速度（d2y2／dt2）について解くと式（１５）が得られる。
【００５８】
【００５９】
車輪垂直加速度推定器131は、式（１５）を用いて前記車輪垂直加速度推定値を算出する。
【００６０】
所望の車輪水平速度である車輪水平速度指令をv2＊（＝x2*の一階微分）とすると、路面203が水平で平らな場合に車輪水平速度dx2／dtが前記車輪水平速度指令v2*となる負荷角度θ1である負荷角度指令θ1＊は式（１６）であらわされる。すなわち、負荷角度指令は、車輪水平加速度指令を重力加速度で除算した値の逆正接である。
【００６１】
【００６２】
車輪水平速度指令生成器101は、車輪水平速度指令v2＊（＝x2*の一階微分）を出力し、負荷角度指令演算器102は、式（１６）を用いて負荷角度指令θ1＊を算出し出力する。
【００６３】
式（１４）と式（１５）を式（１２）に代入すると、式（１７）が得られる。
【００６４】
【００６５】
式（１７）を書き換えると式（１８）とあらわされる。
【００６６】
【００６７】
ただし、ｕは場合分け線形トルクである。
そして、負荷角度θ1が負荷角度指令θ1＊に収束するような、式（１９）に示す場合分け線形トルクｕを考える。
【００６８】
【００６９】
ただし、ｅ＝θ1＊−θ1は、負荷角度追従偏差、
βは、速度比例制御ゲイン、
κは、位置比例制御ゲイン、
γは、減衰パラメータ、
ｓｇｎ（・）は、・が正数のとき＋１、負数のとき−１、零のとき０の値をとるシグナム関数である。
【００７０】
また、ｈ＝ｃ｜θ1＊｜は、フィードバック制御によるチャタリングを抑制するために設けた負荷角度範囲である減衰範囲であり、ｃは、減衰範囲ｈのパラメータである。
【００７１】
ここで、式（１９）に表される場合分け線形トルクｕの意味は、負荷角度θ1と負荷角度指令θ1＊との偏差が小さい場合、具体的には、負荷角度θ1の絶対値が減少する方向において負荷角度θ1と負荷角度指令θ1＊との角度ずれが減衰範囲ｈの範囲内である場合、負荷201には粘性摩擦が減衰パラメータγである動作をさせることにより、負荷角度θ1を負荷角度指令θ1＊に安定して収束させることができることを表す。
また、負荷角度θ1と負荷角度指令θ1＊とのずれが上記以外の場合、負荷201には、剛性が位置比例制御ゲインκで、粘性摩擦が速度比例制御ゲインβであるフィードバック制御をかけることにより、負荷角度θ1を負荷角度指令θ1＊に収束させることができることを表す。
【００７２】
ここで、減衰パラメータγを例えば式（２０）に示すように負荷角度指令θ1＊と負荷角度追従偏差ｅの関数として設定すると好適である。
【００７３】
【００７４】
すなわち、場合分け線形制御部120が負荷角度指令演算器102から負荷角度指令θ1＊を受けると、負荷角度指令θ1＊は減衰範囲演算器121、減衰パラメータ演算器122および場合分け線形トルク演算器123に入力される。
そして、減衰範囲演算器121は、負荷角度指令θ1＊とパラメータcとを用いて、ｈ＝ｃ｜θ1＊｜である減衰範囲を算出する。
この求められた減衰範囲hは制御切替器124に出力される。
【００７５】
また、減衰パラメータ演算器122は、上記式（20）に従って減衰パラメータγを算出し、場合分け線形トルク演算器123に出力する。
【００７６】
場合分け線形トルク演算器123は、減衰パラメータ演算器122から与えられる減衰パラメータγおよび予め設定された速度比例制御ゲインβ、位置比例制御ゲインκを用い、式（19）の場合分け線形トルクuを算出する。
算出した場合分け線形トルクuは、制御切替器124に出力する。
【００７７】
制御切替器124は、場合分け線形トルク演算器123にて求められた場合分け線形トルクuを負荷角度追従偏差ｅおよび減衰範囲hを参照して切替選択する。
制御切替器124にて選択された場合分け線形トルクはトルク指令演算器111に出力される。
【００７８】
また、非線形トルク演算器133は、式（１５）を用いて算出した車輪垂直加速度推定値と、式（１４）を用いて算出した車輪水平速度推定値に基づいて、式（１２）のＮｘ＋Ｎｙである非線形トルクを算出し出力する。
【００７９】
トルク指令演算器111は、場合分け線形トルクｕと非線形トルクＮｘ＋Ｎｙを用いて式（２１）によりトルク指令Ｔｒｅｆを算出し出力する。
【００８０】
【００８１】
ただし、rは車輪半径である。
移動体本体141はトルク指令Ｔｒｅｆにより駆動制御される。
【００８２】
このような構成を備える本第1実施形態によれば、次の効果を奏することができる。
（１）本第1実施形態では、式（１９）に示すように、負荷角度θ1を３つの範囲に分け、それぞれの場合に最適なトルク指令を算出する。
そして、制御切替器124において減衰範囲の内と外とで制御を切り替える。
これにより、負荷角度θ1が負荷角度指令θ1＊の近傍において振動することなく、負荷角度指令θ1＊に滑らかに収束するようにすることができる。
その結果、安定した水平走行を実現させることができる。
【００８３】
（２）減衰パラメータ演算器122を備え、式（２０）により減衰パラメータγを設定するので、減衰パラメータγを一定値に固定する場合に比べ、滑らかに速く負荷角度θ1を負荷角度指令θ1＊に収束させることができる。
【００８４】
（３） 式（１４）の車輪水平速度推定値を用いて移動体本体141を制御するので、車輪202と路面203が相対的にすべる場合にも移動体本体141の車輪水平速度v2（＝x2の一階微分）を車輪水平速度指令v2＊（x2*の一階微分）に一致させることができる。
【００８５】
（４） 式（１５）の車輪垂直加速度推定値を用いて移動体本体141を制御するので、路面203に凹凸があっても車輪202が路面203に接触している間は負荷角度θ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]以降は従来技術では負荷角度が発振しているのに対し、本発明では前記加速度外乱入力後も発振することなく前記負荷角度指令に追従していることが分かる。
【００９１】
そして、本発明による負荷角度が折れ線状の時間変化をしているのは、式（１９）の前記減衰範囲において減衰のみを加える制御を実施しているためであり、前記減衰範囲をもたせることにより前記負荷位置が前記負荷位置指令の近傍において振動的になりにくくなっていることが示された。
【００９２】
また、車輪垂直加速度推定器131において、式（１５）の車輪垂直加速度推定値を算出し、非線形トルク演算器133において、非線形トルクNyを算出することにより、路面203の凹凸のため車輪202に加わる垂直方向の前記加速度外乱を補償できるので、本発明は前記加速度外乱が存在する場合にも前記負荷角度を安定化させることが示された。
【００９３】
図4は、車輪の水平速度変化を示す。
図4において、破線L21は所望の車輪水平速度、実線L20は本発明による車輪水平速度、一点鎖線L22は従来技術による車輪水平速度である。
加速度外乱を入力する0.5[s]までは従来技術も本発明も共に前記所望の車輪水平速度に追従しているが、0.5[s]以降において従来技術は振動的となるのに対し、本発明は振動的とならず前記所望の車輪水平速度に追従していることがわかる。
【００９４】
（変形例1）
上記第1実施形態においては、減衰パラメータγは、上記式（20）に従って減衰パラメータ演算器122により算出したものを用いたが、減衰パラメータγは予め設定された固定値としてもよい。
すなわち、図5に示すように、予め設定された減衰パラメータを減衰パラメータ記憶部125に設定記憶させておき、減衰パラメータ記憶部125から場合分け線形トルク演算器123に減衰パラメータγの値を供給するようにしてもよい。
【００９５】
（変形例2）
また、上記第1実施形態では、最も好適な実施形態として非線形制御部130を有する場合を説明したが、負荷角度指令θ1＊の近傍において振動することなく負荷角度θ1を負荷角度指令θ1＊に滑らかに収束させる安定した制御を実現させる場合、非線形制御部130は無くてもよい。
すなわち、図6に示すように、非線形制御部130を省略した制御装置610であってもよい。
【００９６】
ここで、非線形制御部130を省略して場合分け線形制御部120だけを備える場合には、場合分け線形トルク演算器123にて算出する場合分け線形トルクｕの線形フィードバックトルクのゲインを調整して、外乱となる非線形要素をできる限り抑え込むようなゲインを設定することが好まししい。
【００９７】
ただし、水平ではない凹凸路面の走行や障害物との衝突、車輪の空転などの非線形要素に対応するためには非線形制御部130を合わせもっていた方がよいことはもちろんである。
【００９８】
なお、本発明は上記実施形態に限定されず、本発明の目的を達成できる範囲での変形、改良等は本発明に含まれる。
例えば、式（１９）において、位置Ｐ／速度Ｐ制御は、位置Ｐ／速度ＰＩ制御、位置Ｐ／速度Ｉ−Ｐ制御、位置ＰＩＤ制御など、任意の制御則に置き換えても良いことはもちろんである。
【産業上の利用可能性】
【００９９】
本発明は、路面に凹凸がある場合にも、人や物体と衝突した場合にも、転倒することなく、発振することなく２輪倒立の移動体を所望の水平速度で走行させることができるので、２輪倒立で走行するロボット、電動車椅子、自動搬送装置、災害時の人命救助など狭い場所で作業するロボット、振動に弱い電子部品の組み立て装置などに広く適用できる。
【符号の説明】
【０１００】
100…指令部、101…車輪水平速度指令生成器、102…負荷角度指令演算器、111…トルク指令演算器、120…場合分け線形制御部、121…減衰範囲演算器、122…減衰パラメータ演算器、123…場合分け線形トルク演算器、124…制御切替器、130…非線形制御部、131…車輪垂直加速度推定器、132…車輪水平速度推定器、133…非線形トルク演算器、141…移動体本体、142…状態センサ、125…減衰パラメータ記憶部、110、610…制御装置、201…負荷、202…車輪、203…路面、1001…摩擦推定器、1002…目標状態生成器、1003…状態フィードバックゲイン、1004…倒立ロボット。
【特許請求の範囲】
【請求項１】
車輪を有する駆動手段とリンクを介して前記車輪の上で倒立制御される負荷とを有する移動体本体を倒立させながら走行制御をする制御装置であって、
前記負荷の重心と前記車輪の重心とをつなぐ直線が鉛直線との間になす角を負荷角度とし、
前記負荷角度が所望の前記負荷角度である負荷角度指令の近傍である場合、減衰のみを前記移動体本体に加えるように制御する
ことを特徴とする倒立型移動体の制御装置。
【請求項２】
請求項1に記載の倒立型移動体の制御装置において、
前記負荷角度指令の近傍の幅としての減衰範囲は、前記負荷角度指令の絶対値に所定の係数を乗算して算出する
ことを特徴とする倒立型移動体の制御装置。
【請求項３】
請求項1または請求項2に記載の倒立型移動体の制御装置において、
前記減衰を粘性摩擦とする
ことを特徴とする倒立型移動体の制御装置。
【請求項４】
請求項3に記載の倒立型移動体の制御装置において、
前記負荷角度指令から前記負荷角度を減算した値である負荷角度追従偏差と、前記負荷角度指令と、の関数として前記粘性摩擦である減衰パラメータを算出する
ことを特徴とする倒立型移動体の制御装置。
【請求項５】
請求項4に記載の倒立型移動体の制御装置において、
前記負荷角度指令の絶対値を２で除算した値を前記負荷角度追従偏差から減算し、その絶対値を前記負荷角度指令の絶対値で除算し、さらに定数を乗算することによって前記減衰パラメータを算出する
ことを特徴とする倒立型移動体の制御装置。
【請求項６】
請求項3に記載の倒立型移動体の制御装置において、
前記粘性摩擦である減衰パラメータを一定値とする
ことを特徴とする倒立型移動体の制御装置。
【請求項７】
請求項4から請求項6のいずれかに記載の倒立型移動体の制御装置において、
前記負荷速度と前記減衰パラメータとの乗算値に負号を付した減衰トルクと、位置偏差、速度偏差および加速度偏差の一つ以上に所定ゲインを乗算して得る線形フィードバックトルクと、を算出する場合分け線形トルク演算器と、
前記場合分け線形トルク演算器で算出された前記減衰トルクと前記線形フィードバックトルクとを切り替えて出力する制御切替器と、を備える
ことを特徴とする倒立型移動体の制御装置。
【請求項８】
請求項7に記載の倒立型移動体の制御装置において、
前記制御切替器は、
0 ≦ sgn（θ1＊）・e< h
の場合には、前記減衰トルクを出力し、
それ以外の場合には前記線形フィードバックトルクを出力することを特徴とする倒立型移動体の制御装置。
ただし、ｅ＝θ1＊−θ1であり、θ1＊は負荷角度指令であり、θ1は負荷角度であり、ｓｇｎ（・）は、・が正数のとき＋１、負数のとき−１、零のとき０の値をとるシグナム関数であり、hは前記負荷角度指令の絶対値に所定の係数を乗算して算出する減衰範囲である。
【請求項１】
車輪を有する駆動手段とリンクを介して前記車輪の上で倒立制御される負荷とを有する移動体本体を倒立させながら走行制御をする制御装置であって、
前記負荷の重心と前記車輪の重心とをつなぐ直線が鉛直線との間になす角を負荷角度とし、
前記負荷角度が所望の前記負荷角度である負荷角度指令の近傍である場合、減衰のみを前記移動体本体に加えるように制御する
ことを特徴とする倒立型移動体の制御装置。
【請求項２】
請求項1に記載の倒立型移動体の制御装置において、
前記負荷角度指令の近傍の幅としての減衰範囲は、前記負荷角度指令の絶対値に所定の係数を乗算して算出する
ことを特徴とする倒立型移動体の制御装置。
【請求項３】
請求項1または請求項2に記載の倒立型移動体の制御装置において、
前記減衰を粘性摩擦とする
ことを特徴とする倒立型移動体の制御装置。
【請求項４】
請求項3に記載の倒立型移動体の制御装置において、
前記負荷角度指令から前記負荷角度を減算した値である負荷角度追従偏差と、前記負荷角度指令と、の関数として前記粘性摩擦である減衰パラメータを算出する
ことを特徴とする倒立型移動体の制御装置。
【請求項５】
請求項4に記載の倒立型移動体の制御装置において、
前記負荷角度指令の絶対値を２で除算した値を前記負荷角度追従偏差から減算し、その絶対値を前記負荷角度指令の絶対値で除算し、さらに定数を乗算することによって前記減衰パラメータを算出する
ことを特徴とする倒立型移動体の制御装置。
【請求項６】
請求項3に記載の倒立型移動体の制御装置において、
前記粘性摩擦である減衰パラメータを一定値とする
ことを特徴とする倒立型移動体の制御装置。
【請求項７】
請求項4から請求項6のいずれかに記載の倒立型移動体の制御装置において、
前記負荷速度と前記減衰パラメータとの乗算値に負号を付した減衰トルクと、位置偏差、速度偏差および加速度偏差の一つ以上に所定ゲインを乗算して得る線形フィードバックトルクと、を算出する場合分け線形トルク演算器と、
前記場合分け線形トルク演算器で算出された前記減衰トルクと前記線形フィードバックトルクとを切り替えて出力する制御切替器と、を備える
ことを特徴とする倒立型移動体の制御装置。
【請求項８】
請求項7に記載の倒立型移動体の制御装置において、
前記制御切替器は、
0 ≦ sgn（θ1＊）・e< h
の場合には、前記減衰トルクを出力し、
それ以外の場合には前記線形フィードバックトルクを出力することを特徴とする倒立型移動体の制御装置。
ただし、ｅ＝θ1＊−θ1であり、θ1＊は負荷角度指令であり、θ1は負荷角度であり、ｓｇｎ（・）は、・が正数のとき＋１、負数のとき−１、零のとき０の値をとるシグナム関数であり、hは前記負荷角度指令の絶対値に所定の係数を乗算して算出する減衰範囲である。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【公開番号】特開２０１０−２５７３９９（Ｐ２０１０−２５７３９９Ａ）
【公開日】平成２２年１１月１１日（２０１０．１１．１１）
【国際特許分類】
【出願番号】特願２００９−１０９５９１（Ｐ２００９−１０９５９１）
【出願日】平成２１年４月２８日（２００９．４．２８）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２２年１１月１１日（２０１０．１１．１１）
【国際特許分類】
【出願日】平成２１年４月２８日（２００９．４．２８）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]