(57)【要約】【課題】より安定的に走行することができる倒立型移動体、その制御方法及び制御プログラムを提供すること。【解決手段】倒立型移動体１００は、負荷体２０１と負荷体２０１に回転可能に連結された車輪２０２とを有する移動体本体１４０を備え、車輪２０２の駆動を制御して、負荷体２０１の倒立制御を行う。また、倒立型移動体１００は、移動体本体１４０の状態量を検出する検出手段と、移動体本体１４０が所望の走行状態となる指令値を生成する指令手段と、検出手段により検出された状態量に基づいて、負荷体２０１の重心と車輪２０２の重心との距離である負荷車輪重心間距離ｌを推定する推定手段と、検出手段により検出された状態量と、指令手段により生成された指令値と、推定手段により推定された負荷車輪重心間距離ｌと、に基づいて、車輪２０２を駆動するためのトルク指令値を生成し、倒立制御を行う制御手段と、を備える。【選択図】図１
2011063120.tif 000002
【特許請求の範囲】
【請求項１】
負荷体と該負荷体に回転可能に連結された車輪とを有する移動体本体を備え、前記車輪の駆動を制御して、前記負荷体の倒立制御を行う倒立型移動体であって、
前記移動体本体の状態量を検出する検出手段と、
前記移動体本体が所望の走行状態となる指令値を生成する指令手段と、
前記検出手段により検出された状態量に基づいて、前記負荷体の重心と前記車輪の重心との距離である負荷車輪重心間距離を推定する推定手段と、
前記検出手段により検出された状態量と、前記指令手段により生成された指令値と、前記推定手段により推定された負荷車輪重心間距離と、に基づいて、前記車輪を駆動するためのトルク指令値を生成し、前記倒立制御を行う制御手段と、
を備えることを特徴とする倒立型移動体。
【請求項２】
請求項１記載の倒立型移動体であって、
前記推定手段は、
前記検出手段により検出された移動体本体の状態量に基づいて、前記車輪の水平方向への移動速度である車輪水平速度を算出する水平速度演算部と、
前記検出手段により検出された移動体本体の状態量に基づいて、前記車輪の垂直方向への加速度である車輪垂直加速度を算出する垂直加速度演算部と、
前記水平速度演算部により算出された車輪水平速度と、前記垂直加速度演算部により算出された車輪垂直加速度と、に基づいて、前記移動体本体の運動方程式を前記負荷車輪重心間距離の多項式として表し、該多項式の係数を算出する多項式係数演算部と、
前記多項式係数演算部により算出された前記多項式の係数に基づいて、前記負荷車輪重心間距離を算出する重心間距離演算部と、
を有することを特徴とする倒立型移動体。
【請求項３】
請求項２記載の倒立型移動体であって、
前記多項式係数演算部は、次式により前記多項式の係数ａ２、ａ１、ａ０を算出することを特徴とする倒立型移動体。
2011063120.tif 000024
但し、ｍ１は前記負荷体の質量を表し、ｃＪは前記負荷体の慣性モーメント定数を表し、ｍ２は前記車輪の質量を表し、Ｊ２は前記車輪の慣性モーメントを表し、ｘ２は前記車輪の水平位置を表し、ｙ２は前記車輪の垂直位置を表し、ｒは前記車輪の半径を表し、θ１は前記負荷体の傾斜角度を表し、θ２は前記車輪の回転角度を表し、Ｔｒｅｆは前記トルク指令値を表す。
【請求項４】
請求項２又は３記載の倒立型移動体であって、
前記重心間距離演算部は、前記検出手段により検出された負荷体の傾斜角加速度に応じて、前記負荷車輪重心間距離の算出方法を切換える、ことを特徴とする倒立型移動体。
【請求項５】
請求項４記載の倒立型移動体であって、
前記重心間距離推定部は、次式により負荷車輪重心間距離ｌを算出することを特徴とする倒立型移動体。
2011063120.tif 000025
但し、ａ２、ａ１、ａ０は前記多項式の係数を表し、δ１は負荷車輪重心間距離ｌの前記算出方法を切り替えるための前記負荷体の傾斜角加速度の閾値を表す。
【請求項６】
請求項２乃至５のうちいずれか１項記載の倒立型移動体であって、
前記重心間距離演算部は、前記算出した負荷車輪重心間距離に対してローパスフィルタを適用する、ことを特徴とする倒立型移動体。
【請求項７】
請求項１乃至６のうちいずれか１項記載の倒立型移動体であって、
前記制御手段は、
前記検出手段により検出された状態量と、前記指令手段により生成された指令値と、前記推定手段により推定された負荷車輪重心間距離と、に基づいて、前記移動体本体の車輪を駆動するためのトルク指令値を算出するトルク指令演算部と、
前記推定手段により推定された負荷車輪重心間距離に基づいて、前記トルク指令演算部の制御を調整する制御調整部と、
を有することを特徴とする倒立型移動体。
【請求項８】
請求項７記載の倒立型移動体であって、
前記トルク指令演算部は、前記検出手段により検出された負荷体の傾斜角加速度に応じて、トルク指令値の算出方法を切換える、ことを特徴とする倒立型移動体。
【請求項９】
請求項８記載の倒立型移動体であって、
前記トルク指令演算部は、次式により前記トルク指令値Ｔｒｅｆを算出することを特徴とする倒立型移動体。
2011063120.tif 000026
但し、ｄ１は前記移動体本体の運動方程式における非線形項のうち、前記負荷体の傾斜角速度に比例する項の係数であり、ｄ０は前記移動体本体の運動方程式における非線形項のうち、前記負荷体の傾斜角速度に比例しない項の係数であり、δａは前記算出方法を切り替える際の前記負荷体の傾斜角加速度の閾値であり、ｄｍ０は前記負荷体の傾斜角度θ１と前記車輪の回転角度θ２の関数であり、ｕは前記移動体本体の運動方程式の線形部分を安定化するための線形化トルク指令値であり、σは前記負荷体の傾斜角度θ１が前記指令値に収束する速さを表す収束度である。
【請求項１０】
請求項７乃至９のうちいずれか１項記載の倒立型移動体であって、
前記推定手段が前記負荷車輪重心間距離を推定する間、前記トルク指令演算部は、前記トルク指令値を一定値とする、ことを特徴とする倒立型移動体。
【請求項１１】
請求項１乃至１０のうちいずれか１項記載の倒立型移動体であって、
前記指令手段は、
前記車輪の水平方向への移動速度である水平速度指令値を生成する水平速度指令生成部と、
前記車輪水平速度が前記水平速度指令値に追従するような前記負荷体の傾斜角度である負荷角度指令値を算出する負荷角度指令演算部と、
を有することを特徴とする倒立型移動体。
【請求項１２】
負荷体と該負荷体に回転可能に連結された車輪とを有する移動体本体を備え、前記車輪の駆動を制御して、前記負荷体の倒立制御を行う倒立型移動体の制御方法であって、
前記移動体本体の状態量を検出する工程と、
前記移動体本体が所望の走行状態となる指令値を生成する工程と、
前記検出された状態量に基づいて、前記負荷体の重心と前記車輪の重心との距離である負荷車輪重心間距離を推定する工程と、
前記検出された状態量と、前記生成された指令値と、前記推定された負荷車輪重心間距離と、に基づいて、前記車輪を駆動するためのトルク指令値を生成する工程と、
を含むことを特徴とする倒立型移動体の制御方法。
【請求項１３】
負荷体と該負荷体に回転可能に連結された車輪とを有する移動体本体を備え、前記車輪の駆動を制御して、前記負荷体の倒立制御を行う倒立型移動体の制御プログラムであって、
前記移動体本体が所望の走行状態となる指令値を生成する処理と、
前記移動体本体の状態量に基づいて、前記負荷体の重心と前記車輪の重心との距離である負荷車輪重心間距離を推定する処理と、
前記移動体本体の状態量と、前記生成された指令値と、前記推定された負荷車輪重心間距離と、に基づいて、前記車輪を駆動するためのトルク指令値を生成する処理と、
をコンピュータに実行させる倒立型移動体の制御プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、倒立状態を維持して走行する倒立型移動体、その制御方法及び制御プログラムに関するものである。
【背景技術】
【０００２】
倒立状態を維持する倒立制御を行いつつ走行する倒立型移動体が知られている。例えば、倒立状態を維持する倒立制御を行いつつ、自立的に走行する倒立二輪走行ロボットが開示されている（特許文献１参照）。この倒立二輪走行ロボットにおいては、制御対象である倒立型ロボットを所望の姿勢の近傍で線形化した線形化モデルに基づいて、倒立型ロボットの動作を制御している。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００６−１２３０１４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、上記倒立二輪走行ロボットにおいては、例えば、倒立型ロボットに搭乗する搭乗者がしゃがんだり、或いは積載する荷物が荷崩れしたとき等の重心位置が大きく変化した場合に、倒立型ロボットが振動的な動作をしたり、不安定となり転倒するなどの問題が生じ得る。
【０００５】
本発明は、このような問題点に鑑みてなされたものであり、より安定的に走行することができる倒立型移動体、その制御方法及び制御プログラムを提供することを主たる目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するための本発明の一態様は、負荷体と該負荷体に回転可能に連結された車輪とを有する移動体本体を備え、前記車輪の駆動を制御して、前記負荷体の倒立制御を行う倒立型移動体であって、前記移動体本体の状態量を検出する検出手段と、前記移動体本体が所望の走行状態となる指令値を生成する指令手段と、前記検出手段により検出された状態量に基づいて、前記負荷体の重心と前記車輪の重心との距離である負荷車輪重心間距離を推定する推定手段と、前記検出手段により検出された状態量と、前記指令手段により生成された指令値と、前記推定手段により推定された負荷車輪重心間距離と、に基づいて、前記車輪を駆動するためのトルク指令値を生成し、前記倒立制御を行う制御手段と、を備えることを特徴とする倒立型移動体である。
【０００７】
この一態様において、前記推定手段は、前記検出手段により検出された移動体本体の状態量に基づいて、前記車輪の水平方向への移動速度である車輪水平速度を算出する水平速度演算部と、前記検出手段により検出された移動体本体の状態量に基づいて、前記車輪の垂直方向への加速度である車輪垂直加速度を算出する垂直加速度演算部と、前記水平速度演算部により算出された車輪水平速度と、前記垂直加速度演算部により算出された車輪垂直加速度と、に基づいて、前記移動体本体の運動方程式を前記負荷車輪重心間距離の多項式として表し、該多項式の係数を算出する多項式係数演算部と、前記多項式係数演算部により算出された前記多項式の係数に基づいて、前記負荷車輪重心間距離を算出する重心間距離演算部と、を有していてもよい。
【０００８】
また、この一態様において、前記多項式係数演算部は、下記（１４）式により前記多項式の係数ａ２、ａ１、ａ０を算出してもよい。さらに、この一態様において、前記重心間距離演算部は、前記検出手段により検出された負荷体の傾斜角加速度に応じて、前記負荷車輪重心間距離の算出方法を切換えてもよい。さらにまた、この一態様において、前記重心間距離推定部は、下記（１５）式により負荷車輪重心間距離ｌを算出してもよい。なお、この一態様において、前記重心間距離演算部は、前記算出した負荷車輪重心間距離に対してローパスフィルタを適用してもよい。
【０００９】
さらに、この一態様において、前記制御手段は、前記検出手段により検出された状態量と、前記指令手段により生成された指令値と、前記推定手段により推定された負荷車輪重心間距離と、に基づいて、前記移動体本体の車輪を駆動するためのトルク指令値を算出するトルク指令演算部と、前記推定手段により推定された負荷車輪重心間距離に基づいて、前記トルク指令演算部の制御を調整する制御調整部と、を有していてもよい。さらにまた、この一態様において、前記トルク指令演算部は、前記検出手段により検出された負荷体の傾斜角加速度に応じて、トルク指令値の算出方法を切換えてもよい。さらにまた、この一態様において、前記トルク指令演算部は、下記（１８）又は（２０）式により前記トルク指令値Ｔｒｅｆを算出してもよい。
【００１０】
なお、この一態様において、前記推定手段が前記負荷車輪重心間距離を推定する間、前記トルク指令演算部は、前記トルク指令値を一定値としてもよい。また、この一態様において、前記指令手段は、前記車輪の水平方向への移動速度である水平速度指令値を生成する水平速度指令生成部と、前記車輪水平速度が前記水平速度指令値に追従するような前記負荷体の傾斜角度である負荷角度指令値を算出する負荷角度指令演算部と、を有していてもよい。
【００１１】
他方、上記目的を達成するための本発明の他の態様は、負荷体と該負荷体に回転可能に連結された車輪とを有する移動体本体を備え、前記車輪の駆動を制御して、前記負荷体の倒立制御を行う倒立型移動体の制御方法であって、前記移動体本体の状態量を検出する工程と、前記移動体本体が所望の走行状態となる指令値を生成する工程と、前記検出された状態量に基づいて、前記負荷体の重心と前記車輪の重心との距離である負荷車輪重心間距離を推定する工程と、前記検出された状態量と、前記生成された指令値と、前記推定された負荷車輪重心間距離と、に基づいて、前記車輪を駆動するためのトルク指令値を生成する工程と、を含むことを特徴とする倒立型移動体の制御方法であってもよい。
【００１２】
また、上記目的を達成するための本発明の他の態様は、負荷体と該負荷体に回転可能に連結された車輪とを有する移動体本体を備え、前記車輪の駆動を制御して、前記負荷体の倒立制御を行う倒立型移動体の制御プログラムであって、前記移動体本体が所望の走行状態となる指令値を生成する処理と、前記移動体本体の状態量に基づいて、前記負荷体の重心と前記車輪の重心との距離である負荷車輪重心間距離を推定する処理と、前記移動体本体の状態量と、前記生成された指令値と、前記推定された負荷車輪重心間距離と、に基づいて、前記車輪を駆動するためのトルク指令値を生成する処理と、をコンピュータに実行させる倒立型移動体の制御プログラムであってもよい。
【発明の効果】
【００１３】
本発明によれば、より安定的に走行することができる倒立型移動体、その制御方法及び制御プログラムを提供することができる。
【図面の簡単な説明】
【００１４】
【図１】本発明の一実施形態に係る倒立型移動体の概略的なシステム構成を示すブロック図である。
【図２】本発明の一実施形態に係る倒立型の移動体本体をモデル化したモデル図である。
【図３】推定器により推定される負荷車輪重心間距離の推定値に関するシミュレーション結果の一例を示す図である。
【図４】本発明の一実施形態に係る倒立型移動体と従来技術とにおいて負荷体の傾斜角度θ１の時間変化を比較した図である。
【図５】移動体本体の一具体例である同軸二輪車を概略的に示す図である。
【図６】移動体本体の一具体例である倒立型自律走行ロボットを概略的に示す図である。
【図７】移動体本体の他の具体例を示す模式図である。
【図８】本発明の一実施形態に係る倒立型移動体の制御処理フローの一例を示すフローチャートである。
【発明を実施するための形態】
【００１５】
以下、図面を参照して本発明の一実施形態について説明する。図１は、本発明の一実施形態に係る倒立型移動体の概略的なシステム構成を示すブロック図である。本実施形態に係る倒立型移動体１００は、制御対象としての移動体本体１４０と、移動体本体１４０の状態量を検出する検出器１５０と、移動体本体１４０が所望の走行状態となる指令値を生成する指令器１１０と、移動体本体１４０における後述の負荷車輪重心間距離ｌを推定する推定器１２０と、負荷車輪重心間距離ｌに基づいて、移動体本体１４０の制御を実行する制御器１３０と、を備えている。
【００１６】
移動体本体１４０は、単数又は複数の（例えば、二輪の）車輪を駆動して、任意の位置に移動可能な移動体であり、倒立状態を維持して走行を行う。移動体本体１４０としては、例えば、図５に示すように、搭乗者の重心移動によって走行操作を行う同軸二輪車や、図６に示すように、倒立状態を維持しつつ胴体を傾斜させて走行する倒立型自律走行ロボットなどが適用可能であるが、これに限らず、車輪を用いた走行手段と、走行手段に連結された負荷体とを備え、負荷体の倒立状態を維持する倒立制御を行う任意の車両に適用可能である。
【００１７】
例えば、移動体本体１４０は、図７に示すように、４輪の走行手段７０１上に揺動可能にリンク機構７０２が連結された構成であってもよい。また、リンク機構７０２の上部にカゴ状部材７０３を取付け、このカゴ状部材７０３上に物品等を載置して運搬してもよい。
【００１８】
図２は、図７に示す倒立型の移動体本体１４０をモデル化したモデル図である。図２において、移動体本体１４０は路面２０３上を、倒立状態を維持して走行するものであり、車輪２０２と、車輪２０２に連結された負荷体２０１と、を有している。
【００１９】
負荷体２０１は、例えば、ロボットのボディや、移動体本体１４０に乗車する人または積載する荷物等を含んでいる。また、車輪２０２は、負荷体２０１を乗せた状態で、回転することにより路面２０３との摩擦力を利用して移動することができる。さらに、移動体本体１４０は、例えば、負荷体２０１を前傾又は後傾させたときの負荷体２０１の傾斜角度θ１に応じて、前進又は後進するように構成されている。
【００２０】
図１に示すように、検出手段の一具体例である検出器１５０は、移動体本体１４０に設けられており、移動体本体１４０の状態量を検出することができる。検出器１５０は、例えば、負荷体２０１の傾斜角度θ１を検出する姿勢センサ、車輪２０２の回転角度θ２を検出する回転センサ、などの移動体本体１４０の状態量を検出できる任意のセンサを含んでいる。検出器１５０は、検出した負荷体２０１の傾斜角度θ１、車輪２０２の回転角度θ２等を、移動体本体１４０の状態量として推定器１２０及び制御器１３０に対して出力する。
【００２１】
指令手段の一具体例である指令器１１０は、例えば、搭乗者による走行操作（ハンドル操作、スイッチ操作、重心移動操作等）に応じて、移動体本体１４０が所望の走行状態（前後進、加減速、左右旋回、停止等）となる指令値を生成する。また、指令器１１０は、水平速度指令生成部１１１と、負荷角度指令演算部１１２と、を有している。
【００２２】
水平速度指令生成部１１１は、例えば、搭乗者による走行操作に応じて、車輪２０２の水平方向への移動速度の指令値である水平速度指令値を生成する。水平速度指令生成部１１１は、生成した水平速度指令値を負荷角度指令演算部１１２に対して出力する。
【００２３】
負荷角度指令演算部１１２は、例えば、移動体本体１４０が走行する路面２０３が水平である場合に、後述の水平速度演算部１２１により算出された車輪水平速度が水平速度指令生成部１１１により生成された水平速度指令値に追従するような負荷体２０１の傾斜角度θ１である負荷角度指令値を算出する。負荷角度指令演算部１１２は、算出した負荷角度指令値を制御器１３０に対して出力する。
【００２４】
推定手段の一具体例である推定器１２０は、検出器１５０からの移動体本体１４０の状態量に基づいて、移動体本体１４０の負荷車輪重心間距離ｌを推定する。ここで、負荷車輪重心間距離ｌとは、負荷体２０１の重心と車輪２０２の重心との間の距離を指す。また、推定器１２０は、水平速度演算部１２１と、垂直加速度演算部１２２と、多項式係数演算部１２３と、重心間距離演算部１２４と、を有している。
【００２５】
水平速度演算部１２１は、例えば、検出器１５０からの車輪２０２の回転角度θ２に基づいて、車輪２０２の水平方向への移動速度である車輪水平速度を算出する。水平速度演算部１２１は、算出した車輪水平速度を多項式係数演算部１２３に対して出力する。
【００２６】
垂直加速度演算部１２２は、検出器１５０からの移動体本体１４０の状態量に基づいて、下記（１２）式を用いて車輪２０２の垂直方向への加速度である車輪垂直加速度を算出する。このように、加速度センサを用いることなく車輪垂直加速度を算出することで、コスト低減に繋がる。なお、垂直加速度演算部１２２は、加速度センサにより検出された車輪２０２の加速度を用いて、車輪垂直加速度を求めてもよい。垂直加速度演算部１２２は、算出した車輪垂直加速度を多項式係数演算部１２３に対して出力する。
【００２７】
多項式係数演算部１２３は、水平速度演算部１２１からの車輪水平速度と、垂直加速度演算部１２２からの車輪垂直加速度と、に基づいて、移動体本体１４０の運動方程式を負荷車輪重心間距離ｌの多項式として表し、その多項式の係数である重心間距離係数を算出する。多項式係数演算部１２３は、算出した重心間距離係数を、重心間距離演算部１２４に対して出力する。
【００２８】
重心間距離演算部１２４は、多項式係数演算部１２３からの重心間距離係数を用いて、移動体本体１４０の運動方程式を負荷車輪重心間距離ｌについて解き、負荷車輪重心間距離ｌを算出する。重心間距離演算部１２４は、算出した負荷車輪重心間距離ｌを制御器１３０に対して出力する。
【００２９】
制御手段の一具体例である制御器１３０は、検出器１５０からの状態量と、指令器１１０からの負荷角度指令値と、推定器１２０からの負荷車輪重心間距離ｌと、に基づいて、移動体本体１４０の駆動を制御する。制御器１３０は、制御調整部１３１と、トルク指令演算部１３２と、を有している。
【００３０】
制御調整部１３１は、推定器１２０の重心間距離演算部１２４からの負荷車輪重心間距離ｌに基づいて、トルク指令演算部１３２の制御パラメータや制御則を調整する。
【００３１】
トルク指令演算部１３２は、検出器１５０からの移動体本体１４０の状態量と、指令器１１０からの負荷角度指令値と、推定器１２０からの負荷車輪重心間距離ｌと、に基づいて、例えば、移動体本体１４０が走行する路面２０３が水平な場合に、車輪水平速度が水平速度指令値に追従するようなトルク指令値Ｔｒｅｆを算出する。制御器１３０は、トルク指令演算部１３２により算出されたトルク指令値Ｔｒｅｆに基づいて、移動体本体１４０の車輪２０２の駆動を制御する。
【００３２】
なお、指令器１１０、推定器１２０、及び制御器１３０は、例えば、制御処理、演算処理等を行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される制御プログラム、演算プログラム等が記憶されたＲＯＭ（Read Only Memory）、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）等からなるマイクロコンピュータを中心にしてハードウェア構成することができる。
【００３３】
次に、図２に示す倒立型の移動体本体１４０のモデルを用いて、本実施形態に係る倒立型移動体１００の具体的な倒立制御について説明する。なお、図２において、負荷体２０１の質量をｍ１、負荷体２０１の慣性モーメントをＪ１、車輪２０２の質量をｍ２、車輪２０２の慣性モーメントをＪ２、負荷車輪重心間距離をｌ、車輪２０２の半径をｒ、負荷体２０１の傾斜角度をθ１、車輪２０２の回転角度をθ２、トルク指令演算部１３２が出力するトルク指令値をＴｒｅｆ、とする。
【００３４】
まず、移動体本体１４０の運動エネルギーＴ及びポテンシャルエネルギーＶを、下記（１）及び（２）式を用いて、下記（３）及び（４）式のように表すことができる。ここで、下記（１）及び（２）式において、車輪２０２の水平位置をｘ２、車輪２０２の垂直位置をｙ２、負荷体２０１の水平位置をｘ１、負荷体２０１の垂直位置をｙ１とする。
2011063120.tif 000003
2011063120.tif 000004
2011063120.tif 000005
2011063120.tif 000006
【００３５】
また、上記（３）及び（４）式と、オイラーラグランジュ方程式とを用いて、移動体本体１４０の運動方程式を、下記（５）乃至（８）式のように求めることができる。但し、下記（８）式において、ｇは重力加速度である。
2011063120.tif 000007
2011063120.tif 000008
2011063120.tif 000009
2011063120.tif 000010
【００３６】
さらに、車輪２０２と路面２０３の間の粘性摩擦を考慮して、上記（６）及び（７）式を、下記（９）及び（１０）式のように書き換えることができる。なお、下記（９）及び（１０）式において、Ｄは粘性摩擦係数とする。
2011063120.tif 000011
2011063120.tif 000012
【００３７】
また、上記（９）及び（１０）式を用いて、下記（１１）式を求めることができる。
2011063120.tif 000013
【００３８】
さらに、上記（５）式から上記（１１）式を減算し、下記（１２）式を得ることができる。
2011063120.tif 000014
【００３９】
なお、上記（１２）式において、Ｎｘは車輪２０２の水平位置ｘ２をパラメータとした非線形項であり、Ｎｙは車輪２０２の垂直位置ｙ２をパラメータとした非線形項である。また、負荷体２０１の慣性モーメントＪ１は、負荷車輪重心間距離ｌの２乗に比例する部分とそれ以外の和として表すことができるので、負荷体２０１の慣性モーメント定数ｃＪを用いて、下記（１３）式のように表すことができる。
2011063120.tif 000015
【００４０】
さらに、上記（１３）式を上記（１２）式に代入し、下記（１４）式を得ることができる。ここで、トルク指令値Ｔｒｅｆは、負荷車輪重心間距離ｌの関数でないものとする。
2011063120.tif 000016
【００４１】
多項式係数演算部１２３は、検出器１５０からの負荷体２０１の傾斜角度θ１と、車輪２０２の回転角度θ２と、に基づいて、車輪２０２の水平位置ｘ２及び垂直位置ｙ２を算出する。そして、多項式係数演算部１２３は、検出器１５０からの負荷体２０１の傾斜角度θ１及び車輪２０２の回転角度θ２と、算出した車輪２０２の水平位置ｘ２及び垂直位置ｙ２と、に基づいて、上記（１４）式を用いて、重心間距離係数ａ０、ａ１、ａ２を算出する。多項式係数演算部１２３は、算出した重心間距離係数ａ０、ａ１、ａ２を、重心間距離演算部１２４に対して出力する。
【００４２】
重心間距離演算部１２４は、多項式係数演算部１２３からの重心間距離係数ａ０、ａ１、ａ２を用いて、移動体本体１４０の運動方程式である上記（１４）式を負荷車輪重心間距離ｌについて解き、下記（１５）式を求め、この（１５）式に基づいて、負荷車輪重心間距離ｌを算出する。
2011063120.tif 000017
【００４３】
なお、上記（１５）式において、δ１は負荷車輪重心間距離ｌの算出方法（推定式）を切り替えるための、負荷体２０１の傾斜角加速度（傾斜角度θ１の２回時間微分）の閾値である負荷体加速度切替値である。上述したように、重心間距離演算部１２４は、上記（１５）式において、負荷体２０１の傾斜角加速度（以下、負荷体加速度と称す）に応じて、負荷車輪重心間距離ｌの算出方法を切り替える。これにより、負荷体加速度が小さい場合でも精度よく負荷車輪重心間距離ｌを算出することができる。
【００４４】
また、重心間距離演算部１２４は、下記（１６）式に示すように、算出した負荷車輪重心間距離ｌ０に対してローパスフィルタＬＰＦ（）を適用することで、負荷車輪重心間距離ｌを算出してもよい。これにより、負荷体加速度が負荷体加速度切替値δ１を通過する時に、負荷車輪重心間距離ｌは滑らかに変化し、結果として制御調整部１３１によって制御パラメータが滑らかに変化する。したがって、例えば、移動体本体１４０に乗車した搭乗者がしゃがんだり、或いは積載した荷物が荷崩れした場合でも、移動体本体１４０が振動することなく滑らかに走行を継続することができる。すなわち、移動体本体１４０は、より安定的に走行することができる。
2011063120.tif 000018
【００４５】
なお、上記（１６）式において、ｌ０は負荷車輪重心間距離ｌの調整前の値であり、ＬＰＦ（）はローパスフィルタ関数である。このように、重心間距離演算部１２４は、負荷車輪重心間距離ｌ０に対してローパスフィルタを適用することにより、上記（１６）式の切替時においても負荷車輪重心間距離ｌが滑らかに変化するため、移動体本体１４０の動作をより滑らかに継続させることができる。
【００４６】
重心間距離演算部１２４は、上述のようにして求めた負荷車輪重心間距離ｌを、制御器１３０に対して出力する。
【００４７】
制御器１３０は、重心間距離演算部１２４からの負荷車輪重心間距離ｌを用いて、移動体本体１４０の駆動を制御する。ここで、上記（１２）式を用いて、非線形トルクＮｘ＋Ｎｙを下記（１７）式のように表すことができる。なお、下記（１７）及び（１８）式において、ｄ１及びｄ０は負荷体２０１の傾斜角度θ１及び車輪２０２の回転角度θ２の関数である。また、ｄ１は、移動体本体１４０の運動方程式における非線形項のうち、負荷体２０１の傾斜角速度に比例する項の係数であり、ｄ０は、移動体本体１４０の運動方程式における非線形項のうち、負荷体２０１の傾斜角速度に比例しない項の係数である。
2011063120.tif 000019
【００４８】
トルク指令演算部１３２は、下記（１８）式を用いて、移動体本体１４０の車輪２０２を駆動するためのトルク指令値Ｔｒｅｆを算出する。
2011063120.tif 000020
【００４９】
なお、上記（１８）式において、δａは、上記算出式（算出方法）を切り替える際の負荷体加速度の閾値であり、ｄｍ０は、負荷体２０１の傾斜角度θ１と車輪２０２の回転角度θ２の関数である。また、σは、負荷体２０１の傾斜角度θ１が負荷角度指令演算部１１２により算出された負荷角度指令値に収束する速さを表す収束度であり、ｕは、移動体本体１４０の運動方程式の線形部分であり、下記（１９）式を収束度σで安定化するための線形化トルク指令値である。
2011063120.tif 000021
【００５０】
制御調整部１３１は、上記（１８）式においてδaの判定を行い、トルク指令値Ｔｒｅｆを求めるための上記算出式の切替を指示する。また、制御調整部１３１は、重心間距離演算部１２４からの負荷車輪重心間距離ｌを用いて、トルク指令演算部１３２の比例制御等における制御ゲイン（負荷車輪重心間距離ｌの関数）の設定を行う。
【００５１】
このように、トルク指令演算部１３２は、上記（１８）式を用いて、負荷体加速度の大きさに応じてトルク指令値Ｔｒｅｆの上記算出式を切替える。これにより、例えば、移動体本体１４０が加減速していない時において、トルク指令値Ｔｒｅｆの算出が簡単となり、ＣＰＵの負荷を軽減でき、電気ハードの発熱量等も低減することができる。
【００５２】
なお、トルク指令演算部１３２は、上記（１８）式の代わりに下記（２０）式を用いて、トルク指令値Ｔｒｅｆを算出してもよい。
2011063120.tif 000022
【００５３】
制御器１３０は、トルク指令演算部１３２により算出されたトルク指令値Ｔｒｅｆに基づいて、移動体本体１４０に設けられたモータ等の駆動手段を制御することで、車輪２０２の駆動を制御する。
【００５４】
上述のように、制御器１３０は、（１８）又は（２０）式のように、負荷車輪重心間距離ｌを用いてトルク指令値Ｔｒｅｆを算出することにより、例えば、移動体本体１４０の搭乗者がしゃがんだり立ったりして姿勢を変化させ、あるいは移動体本体１４０に積載した荷物等が荷崩れして、その重心位置が大きく変化した場合でも、負荷体２０１の傾斜角度θ１は常に収束度σで負荷角度指令値に収束する。したがって、移動体本体１４０を安定的に走行させることができる。
【００５５】
なお、推定器１２０が負荷車輪重心間距離ｌを推定する間、制御器１３０のトルク指令演算部１３２は、トルク指令値Ｔｒｅｆを一定値として算出してもよい。これにより、負荷車輪重心間距離ｌがトルク指令値Ｔｒｅｆに含まれるノイズによって影響を受けるのを低減でき、負荷車輪重心間距離ｌの推定精度を向上させることができる。例えは、トルク指令演算部１３２は、推定器１２０が負荷車輪重心間距離ｌの推定を開始したときのトルク指令値Ｔｒｅｆを、上記一定値としてもよく、実質的に０を一定値としてもよい。
【００５６】
次に、図２に示す倒立型の移動体本体１４０のモデルを用いて行ったシミュレーションについて、説明する。
【００５７】
なお、本シミュレーションにおいて、ｍ１＝７０[ｋｇ]、ｒｌ＝０．６［ｍ］、ｃＪ＝１／１２＊ｍ１＊４[ｋｇ]、ｃ０＝１／１２＊ｍ１＊３＊ｒｌ２[ｋｇ]、ｌ０＝０．９[ｍ]、Ｊ１＝（ｍ１＋ｃＪ）＊ｌ２＋ｃ０、ｒ＝０．１[ｍ]、ｍ２＝１５[ｋｇ]、Ｊ２＝ｍ２＊ｒ２／２[ｋｇ＊ｍ２]、ｇ＝９．８[ｍ／ｓ２]、Ｔ＝１×１０−３[ｓ]、Ｋｐ＝１０［ｓ−１]、Ｋｖ＝１０＊２＊π[ｓ−１]、Ｋｖｊ＝Ｋｖ＊Ｊ１、とする。
【００５８】
また、ｍ１は負荷体２０１の質量、ｒｌは搭乗者の肩幅の半分、ｃＪは搭乗者を円筒に近似したときの上記（１３）式における負荷車輪重心間距離ｌの２乗に比例する項の係数、ｃ０は搭乗者を円筒に近似したときの上記（１３）式における負荷車輪重心間距離ｌによらない項、ｌ０は初期状態での負荷車輪重心間距離ｌ、Ｊ１は負荷体２０１の慣性モーメント、ｍ２は車輪２０２の質量、ｒは車輪２０２の半径、Ｊ２は車輪２０２の慣性モーメント、ｇは重力加速度、Ｔは制御周期、Ｋｐは位置比例制御ゲイン、Ｋｖは正規化速度比例制御ゲイン、Ｋｖｊは速度比例制御ゲイン、である。
【００５９】
さらに、本シミュレーションにおいて、トルク指令演算部１３２は、位置比例制御ゲインＫｐと速度比例制御ゲインＫｖｊをパラメータとした位置比例／速度比例制御により、上記（１９）式の線形化トルク指令値ｕを算出して、移動体本体１４０の車輪２０２を駆動制御する。以下、負荷車輪重心間距離ｌを下記（２１）式のように変化させた場合において、本発明に係る倒立制御を適用した場合と適用しない場合との比較を示す。
【００６０】
2011063120.tif 000023
【００６１】
図３は、推定器により推定される負荷車輪重心間距離の推定値に関するシミュレーション結果の一例を示す図である。なお、図３において、実線は負荷車輪重心間距離ｌの真値を示し、破線は推定器１２０により推定された負荷車輪重心間距離ｌの推定値を示す。図３に示すように、負荷車輪重心間距離ｌの推定値は、負荷車輪重心間距離ｌの真値に対して追従し、これら推定値と真値は重複しており、良好な結果が得られていることが分かる。
【００６２】
図４は、本実施形態に係る倒立型移動体１００と従来技術とにおいて、負荷体２０１の傾斜角度θ１の時間変化を比較した図である。なお、図４において、実線（１）は本実施形態に係る負荷体２０１の傾斜角度θ１の時間変化を示し、一点鎖線（２）は従来技術に係る負荷体の傾斜角度θ１の時間変化を示し、点線（３）は指令器１１０の負荷角度指令値を示している。また、この従来技術においては、同一の制御ゲインを用いて位置比例／速度比例制御のみが実行されているものとする。
【００６３】
図４に示すように、従来技術（一点鎖線（２））においては、約３[ｓ]後に負荷体の傾斜角度θ１が発散し、移動体本体が転倒していることが分かる。一方で、本実施形態（実線（１））においては、負荷体２０１の傾斜角度θ１は０．１ｒａｄ以下に抑えられおり、、移動体本体１４０は、指令器１１０からの負荷角度指令値に応じて、常時安定的に動作していることが分かる。このように、本実施形態に係る倒立型移動体１００においては、移動体本体１４０の搭乗者や積載荷物等の重心位置が大きく変化した場合でも、振動的な動作をすることなく、また、不安定となり転倒することなく、安全に走行または停止の状態を継続させることができる。
【００６４】
次に、本実施形態に係る倒立型移動体１００の制御方法について、詳細に説明する。図８は、本実施形態に係る倒立型移動体の制御処理フローの一例を示すフローチャートである。
【００６５】
検出器１５０は、負荷体２０１の傾斜角度θ１、車輪２０２の回転角度θ２等の移動体本体１４０の状態量を検出し（ステップＳ１０１）、推定器１２０及び制御器１３０に対して出力する。
【００６６】
また、指令器１１０の水平速度指令生成部１１１は、搭乗者による走行操作に応じて、車輪２０２の水平方向への移動速度である水平速度指令値を生成し（ステップＳ１０２）、生成した水平速度指令値を負荷角度指令演算部１１２に対して出力する。
【００６７】
次に、負荷角度指令演算部１１２は、水平速度指令生成部１１１からの水平速度指令値に基づいて、負荷角度指令値を算出し（ステップＳ１０３）、算出した負荷角度指令値を制御器１３０に対して出力する。
【００６８】
一方、推定器１２０の水平速度演算部１２１は、例えば、検出器１５０からの車輪２０２の回転角度θ２に基づいて車輪水平速度を算出し（ステップＳ１０４）、算出した車輪水平速度を多項式係数演算部１２３に対して出力する。
【００６９】
また、垂直加速度演算部１２２は、上記（１２）式を用いて車輪垂直加速度を算出し（ステップＳ１０５）、算出した車輪垂直加速度を多項式係数演算部１２３に対して出力する。
【００７０】
その後、多項式係数演算部１２３は、水平速度演算部１２１からの車輪水平速度と、垂直加速度演算部１２２からの車輪垂直加速度と、に基づいて、重心間距離係数を算出し（ステップＳ１０６）、算出した重心間距離係数を、重心間距離演算部１２４に対して出力する。
【００７１】
そして、重心間距離演算部１２４は、多項式係数演算部１２３からの重心間距離係数に基づいて、負荷車輪重心間距離ｌを算出し（ステップＳ１０７）、算出した負荷車輪重心間距離ｌを制御器１３０に対して出力する。
【００７２】
次に、制御器１３０の制御調整部１３１は、重心間距離演算部１２４からの負荷車輪重心間距離ｌに基づいて、トルク指令演算部１３２の制御パラメータや制御則を調整する（ステップＳ１０８）。
【００７３】
トルク指令演算部１３２は、検出器１５０からの移動体本体１４０の状態量と、指令器１１０からの負荷角度指令値と、重心間距離演算部１２４からの負荷車輪重心間距離ｌと、に基づいて、上記（１８）又は（２０）式を用いて、トルク指令値Ｔｒｅｆを算出する（ステップＳ１０９）。
【００７４】
制御器１３０は、トルク指令演算部１３２により算出されたトルク指令値Ｔｒｅｆに基づいて、移動体本体１４０の車輪２０２の駆動を制御する（ステップＳ１１０）。
【００７５】
以上、本実施形態に係る倒立型移動体１００において、推定器１２０は負荷車輪重心間距離ｌを推定し、制御器１３０はこの負荷車輪重心間距離ｌに基づいてトルク指令値Ｔｒｅｆを算出し、倒立制御を行う。これにより、例えば、移動体本体１４０の搭乗者が姿勢を変更した場合や、積載荷物などが崩れた場合等の重心位置が大きく変化した場合でも、振動的な動作をすることなく、また不安定となり転倒することなく、安全に走行または停止の状態を継続させることができる。すなわち、倒立型移動体１００はより安定的に走行することができる。
【００７６】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。また、上述の実施形態では、本発明をハードウェアの構成として説明したが、本発明は、これに限定されるものではない。本発明は、任意の処理を、ＣＰＵ（Central Processing Unit）にコンピュータプログラムを実行させることにより実現することも可能である。この場合、コンピュータプログラムは、記録媒体に記録して提供することも可能であり、また、インターネットその他の通信媒体を介して伝送することにより提供することも可能である。また、記憶媒体には、例えば、フレキシブルディスク、ハードディスク、磁気ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＤＶＤ、ＲＯＭカートリッジ、バッテリバックアップ付きＲＡＭメモリカートリッジ、フラッシュメモリカートリッジ、不揮発性ＲＡＭカートリッジ等が含まれる。また、通信媒体には、電話回線等の有線通信媒体、マイクロ波回線等の無線通信媒体等が含まれる。
【産業上の利用可能性】
【００７７】
本発明は、例えば、２輪倒立で走行するロボット、電動車椅子、自動搬送装置、災害時の人命救助など狭い場所で作業するロボット、振動に弱い電子部品の組立装置などに広く適用できる。
【符号の説明】
【００７８】
１００    倒立型移動体
１１０    指令器
１１１    水平速度指令生成部
１１２    負荷角度指令演算部
１２０    推定器
１２１    水平速度演算部
１２２    垂直加速度演算部
１２３    多項式係数演算部
１２４    重心間距離演算部
１３０    制御器
１３１    制御調整部
１３２    トルク指令演算部
１４０    移動体本体
１５０    検出器
２０１    負荷体
２０２    車輪
２０３    路面
【図１】
2011063120.tif 000027
【図２】
2011063120.tif 000028
【図３】
2011063120.tif 000029
【図４】
2011063120.tif 000030
【図５】
2011063120.tif 000031
【図６】
2011063120.tif 000032
【図７】
2011063120.tif 000033
【図８】
2011063120.tif 000034