JP4646640B2 - 移動台車及び移動台車の制御方法 - Google Patents

Description

本発明は倒立振子型の移動台車に関し、詳しくは、台車進行方向に移動可能な慣性体（制御アーム等）を備えた移動台車に関する。

倒立振子型の移動台車は、台車全体の重心が車軸より上方に位置するため、そのままでは倒立状態を維持することができない。このため、台車が倒立状態を維持するよう倒立制御が行われる。かかる移動台車では、台車が停止した状態から移動を開始する場合や、台車が運動している状態から停止する場合には（すなわち、台車が加減速する場合には）、台車に慣性力が作用し、車体を車軸周りに回転させようとする。このため、台車が倒立状態を維持するためには、この慣性力を補償する補償トルクを発生させる必要がある。
移動台車を高速で移動させるためには移動台車の加減速度を大きくする必要があるが、移動台車の加減速度が大きくなると台車に作用する慣性力も大きくなる。台車に作用する慣性力が大きくなると、台車の加減速に同期して補償トルクを迅速に発生させることができず、倒立状態を維持することが困難となる。そこで、台車の高速移動を可能とするために、台車に台車進行方向に移動可能な慣性体（アーム）を設け、この慣性体によって加減速時の慣性力を補償することが提案されている（例えば、非特許文献１等）。
非特許文献１の移動台車では、車体に車軸が回転可能に取付けられ、この車軸の両端にそれぞれ車輪が固定されている。車体には台車進行方向に回動可能な制御アームが取付けられている。制御アームはモータによって駆動され、制御アームの位置はエンコーダによって検出される。この移動台車では、台車の加減速に同期して制御アームが駆動される。このため、慣性力に応じた補償トルクが速やかに発生し、台車の高速移動化が図られている。
平林、山藤、「可変構造型移動ロボットの制御に関する研究」、日本機械学会論文集（Ｃ編）５８巻５５２号（１９９２−８）ｐ．２５０１−２５０６

しかしながら、非特許文献１に開示された移動台車では、移動台車の運動方程式を厳密に導出し、この運動方程式に基づいて制御アームの制御を行っている。このため、制御アームの目標値（目標傾斜角度）が、車輪の加減速に応じた目標値や、車体の傾斜角度に応じた目標値や、制御アームの回転角度に基づく目標値等の和として与えられる。従って、多くのゲインが必要となり、それらゲインの調整が面倒であるといった欠点を有していた。また、制御アームの回転角度に基づいて制御アームの目標値を算出しているため、制御アームが振動すると、その振動がなかなか減衰せず、制御が不安定になるという問題も有していた。

本発明は上述した実情に鑑みてなされたものであり、その目的は、台車進行方向に移動可能に設けた慣性体の制御を単純な制御構成によって行うことを可能とし、これによりゲイン調整を容易に行うことができ、かつ、慣性体に振動が生じても速やかに減衰し、安定して倒立姿勢を維持できる技術を提供することである。

上記課題を解決するために、本願の移動台車は、車軸と、車軸上に配置された２以上の車輪と、車軸に対し回動可能に支持された車体を備える倒立振子型の移動台車である。この移動台車は、車体に対して少なくとも台車進行方向に移動可能に取付けられた慣性体と、車輪を駆動する第１アクチュエータと、慣性体を駆動する第２アクチュエータと、車輪の位置、速度及び加速度の少なくとも１つを検出する第１検出手段と、車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第２検出手段と、車体に対する慣性体の位置、速度及び加速度の少なくとも１つを検出する第３検出手段を有する。そして、車輪の位置、速度及び加速度の少なくとも１つに関して目標値を入力する入力手段と、車輪の位置、速度及び加速度の少なくとも１つが入力された目標値となり、かつ、車体が倒立状態を維持するように、入力された目標値と第１及び第２検出手段により検出された検出値とに基づいて、第１アクチュエータの制御指令値を算出する第１コントローラを備える。さらに、入力された車輪の目標値のみに基づいて、車体に対する慣性体の位置、速度及び加速度の少なくとも１つに関して目標値を算出する手段と、算出された目標値と第３検出手段により検出された検出値とに基づいて第２アクチュエータの制御指令値を算出する第２コントローラを備える。
この移動台車では、慣性体の目標値が車輪の目標値（すなわち、車輪の目標位置、目標速度又は目標加速度）のみに基づいて算出される。従って、従来と比較してゲインの数が少なくなり、ゲイン調整を容易に行うことができる。また、慣性体の目標値が慣性体及び台車の現在の状態（慣性体の位置、速度又は加速度、並びに車体の傾き角度、傾き角速度）に基づいて算出されないため、慣性体及び車体が振動しても、その振動の影響を受けることはなく、安定した制御を行うことができる。

前記目標値算出手段は、入力手段によって入力された目標値から決定される車輪の目標加減速度に基づく加速トルクが慣性体の重力トルクと釣り合うように慣性体の目標位置を算出することが好ましい。
このような構成によると、（１）車輪の目標加減速度に基づく加速トルクと（２）慣性体に作用する重力トルクが釣り合うように慣性体が駆動されるため、車輪の加減速中に車体が傾くことが抑制され、倒立状態を安定して保つことができる。

前記第１アクチュエータは、（１）第１コントローラによって算出される制御指令値と、（２）入力手段によって入力された目標値から決定される車輪の加減速度を補償する加減速補償値との和に基づいて駆動されることが好ましい。
このような構成によると、第１コントローラによって算出される制御指令値に車輪の加減速度を補償する加減速補償値が加えられ、その制御指令値に基づいて第１アクチュエータが車輪を駆動する。このため、車輪の加減速に必要なトルクがフィードフォワード補償されて車輪に加えられるため、車輪は速やかに加減速し、より遅れの小さい動作が可能となる。

前記第１コントローラは、車体に対して慣性体が所定の位置で固定されていると仮定して導出した運動方程式に基づいて設計することができる。慣性体が車体に固定されていると仮定することで、第１コントローラの設計を簡易に行うことができる。
この場合において第１コントローラは、外乱に対して安定倒立可能なようにロバスト性を有することが好ましい。第１コントローラにロバスト性を付与することで、慣性体が車体に固定されていると仮定して第１コントローラを設計しても、車体は安定して倒立した状態を保つことができる。

また、本願は倒立振子型の移動台車を制御するための新たな制御方法を提供する。
すなわち、本願の移動台車の制御方法は、車軸と、車軸上に配置された２以上の車輪と、車軸に対し回動可能に支持された車体と、車体に対して少なくとも台車進行方向に移動可能に取付けられた慣性体と、車輪を駆動する第１アクチュエータと、慣性体を駆動する第２アクチュエータと、第１アクチュエータと第２アクチュエータに制御指令値を出力する制御コンピュータと、車輪の位置、速度及び加速度の少なくとも１つを検出する第１検出手段と、車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第２検出手段と、車体に対する慣性体の位置、速度及び加速度の少なくとも１つを検出する第３検出手段とを備える倒立振子型の移動台車の制御方法に関する。
この制御方法は、（１）第１、第２及び第３検出手段で検出される検出値を読込む工程と、（２）車輪の位置、速度及び加速度の少なくとも１つに関して目標値を読込む工程と、（３）車輪の位置、速度及び加速度の少なくとも１つが読込んだ目標値となり、かつ、車体が倒立状態を維持するように、読込んだ目標値と、第１及び第２検出手段より読込んだ検出値とに基づいて、第１アクチュエータの制御指令値を算出する工程と、（４）読込んだ目標値のみに基づいて、車体に対する慣性体の位置、速度及び加速度の少なくとも１つに関して目標値を算出する工程と、（５）算出された目標値と第３検出手段より読込んだ検出値とに基づいて第２アクチュエータの制御指令値を算出する工程と、（６）算出された各制御指令値を第１及び第２アクチュエータのそれぞれに出力する工程と、を有する。そして、上記各工程を制御コンピュータで繰返し実行することで移動台車を制御することを特徴としている。

本発明を具現化した一実施形態に係る移動台車について図面を参照して説明する。図１は本実施形態の移動台車の全体構成を模式的に示す図である。
図１に示すように本実施形態の移動台車は、台車本体１０（質量ｍ_３）と、台車本体１０に組み付けられた制御アーム１１（質量ｍ_４）を備えている。台車本体１０にはスライド軸１１ａが配設されている。スライド軸１１ａは、車輪１２，１３の回転軸線に対して略垂直に配されている。制御アーム１１は、スライド軸１１ａに案内され、台車進行方向（台車並進方向）にスライド移動可能となっている。具体的には、スライド軸１１ａがモータ３１（図３に図示）によって回転駆動されると、制御アーム１１がスライド軸１１ａ上を台車進行方向にスライド移動するようになっている。スライド軸１１ａの回転量は、モータ３１に取付けられたエンコーダ３１ａ（図３に図示）によって検出される。従って、エンコーダ３１ａの出力から、台車本体１０に対する制御アーム１１の位置（スライド量）が検出される。なお、本実施形態において制御アーム１１の位置は、車輪１２，１３の回転軸中心Ｃと台車本体１０の重心Ｇ_３とを結ぶ直線Ａと直交する方向で、その直線Ａからのスライド量ｓによって表すものとする。

図２は台車本体１０の概略構成を示す斜視図である。なお、図２ではスライド軸１１ａ及び制御アーム１１等の図示を省略している。
図２に示すように、台車本体１０の下部には車輪１２，１３が配設されている。両車輪１２，１３は同一回転軸線上に配され、この回転軸線に対して直交する方向に台車本体１０が傾動可能となっている。右車輪１２にはモータ１４が接続されており、左車輪１３にはモータ１５が接続されている。モータ１４，１５には、それぞれのモータの回転角度を検出するエンコーダ１４ａ，１５ａ（図３に図示）が取付けられている。図中の１９は１軸ジャイロセンサであり、車軸とは直交する方向（すなわち、台車本体１０の傾動方向）に配置されている。従って、ジャイロセンサ１９によって台車本体１０の傾斜角速度が検出される。

台車本体１０の収納部には、両モータ１４，１５を駆動するためのモータドライバ１６、制御コンピュータ２０およびバッテリ１８が搭載されている。制御コンピュータ２０は、ジャイロセンサ１９の出力、モータ１４，１５のエンコーダ出力等に基づいてモータ１４，１５のトルク指令値を算出する。モータ１４，１５は、制御コンピュータ２０で算出されたトルク指令値に基づいて駆動される。
台車本体１０の上部１０ａにはスライド軸１１ａが配設され、このスライド軸１１ａに制御アーム１１が取付けられる。また、台車本体１０の上部１０ａには、モータ３１を回転駆動するモータドライバ１７（図３に図示）等も配設されている。

次に、本実施形態に係る移動台車の制御系について説明する。図３は、移動台車の制御系の構成を示すブロック図である。
図３に示すように、移動台車の制御は制御コンピュータ２０を中心に行われる。制御コンピュータ２０は、ＣＰＵ，ＲＯＭ，ＲＡＭ等によって構成される。制御コンピュータ２０は、ＲＯＭに格納された制御プログラムを実行することで、台車並進方向に関する制御指令値を算出する台車並進方向制御指令値算出手段２２（以下、単に第１制御指令値算出手段という）と、台車回転方向に関する制御指令値を算出する台車回転方向制御指令値算出手段２６（以下、単に第２制御指令値算出手段という）と、両制御指令値算出手段２２，２６等に目標値を入力する目標値入力手段２４と、両制御指令値算出手段２２，２６で算出された制御指令値を加算する制御指令値加算手段２８と、制御アーム１１の制御目標値を算出する制御目標値算出手段２７と、制御アーム１１に関する制御指令値を算出する制御指令値算出手段２９（以下、第３制御指令値算出手段という）として機能する。制御コンピュータ２０によって構成される各手段２２，２４，２６，２８，２７，２９については後で詳述する。
制御コンピュータ２０にはジャイロセンサ１９が接続され、ジャイロセンサ１９の出力（台車本体１０の傾斜角速度）が入力するようになっている。また、制御コンピュータ２０には、モータ駆動回路１６ａ（モータドライバ１６の一部）、モータ駆動回路１６ｂ（モータドライバ１６の一部）及びモータ駆動回路１７が接続されている。モータ駆動回路１６ａはモータ１４と接続され、制御コンピュータ２０からのトルク指令値に基づいてモータ１４を駆動する。モータ駆動回路１６ｂはモータ１５と接続され、制御コンピュータ２０からのトルク指令値に基づいてモータ１５を駆動する。同様に、モータ駆動回路１７はモータ３１と接続され、制御コンピュータ２０からのトルク指令値に基づいてモータ３１を駆動する。モータ１４，１５，３１の各エンコーダ１４ａ，１５ａ，３１ａは制御コンピュータ２０に接続され、エンコーダ１４ａ，１５ａ，３１ａからの出力（モータ１４，１５，３１の各回転角度）が制御コンピュータ２０に入力するようになっている。

次に、第１制御指令値算出手段２２について説明する。第１制御指令値算出手段２２は、台車並進方向に関する制御を行うためのトルク指令値を算出する。詳しくは、ジャイロセンサ１９の出力と、目標値入力手段２４によって入力される台車並進方向に関する目標値とエンコーダ１４ａ，１５ａの出力から決まる台車の現在値との偏差を入力として、その偏差を小さくすると共に台車本体１０が倒立を維持するようにモータ１４，１５のトルク指令値を算出する。
本実施形態では、Ｈ∞制御理論を用いて第１制御指令値算出手段２２を設計している。Ｈ∞制御理論を用いることで、第１制御指令値算出手段２２は外乱に対して安定倒立可能なロバスト性を有することとなる。なお、本実施形態ではＨ∞制御理論を用いたが、Ｈ∞制御理論以外の制御理論（例えば、Ｈ_２制御理論、μ−設計法、ＰＩＤ制御等）を用いて第１制御指令値算出手段２２を設計することもできる。また、第１制御指令値算出手段２２はロバスト性を有しないものでもよく、例えば、現代制御理論等の制御理論を用いて設計することもできる。

第１制御指令値算出手段２２の設計手順の一例を説明する。まず、移動台車を真横から見て、１輪の倒立振子としてモデル化する（図４参照）。本設計例では、台車本体１０と制御アーム１１を１質点系としてモデル化している。すなわち、制御アーム１１は台車本体１０に対して所定の位置（図１においてｓ＝０）で固定されているものと仮定し、台車本体１０と制御アーム１１の合成重心に、台車本体１０と制御アーム１１の全質量が存在するものとしてモデル化している。図４中、ｍ１は台車本体１０と制御アーム１１の合成質量、Ｊ１は台車本体１０と制御アーム１１の合成重心周りのイナーシャ、ｍ２は車輪の質量、Ｊ２は車輪の軸周りのイナーシャである。また、車軸から合成重心までの距離をｌとしている。これら各パラメータｍ１，J１，ｍ２，Ｊ２，ｌは、計算または実測により求めることができる。また、鉛直方向からの傾きをηとし、車輪の回転角度をθ１とする。
そして、図４に示す１輪の倒立振子に対し運動方程式を作成する。すなわち、この制御モデルに対してトルク指令値ｕが入力されるとして運動方程式を作成すると、その運動方程式は下記に示す式で表される。

次いで、上述した運動方程式中のηが小さいとして線形化し、行列表示を行うと次の式が導かれる。

したがって、上記の式から次の状態方程式が導出される。

ここで、上記制御系で観測される観測量ｙ_ｇは、鉛直方向からの車体の傾きηの１階微分ｄη／ｄｔと、車体に対する車輪の回転角度（θ１−η）と、この回転角度（θ１−η）の１階微分ｄ（θ１−η）／ｄｔとする。すなわち、観測量ｙ_ｇは、次に示す式で表される。

上述した手順でモデル化された制御系の全体構成を図６に示す。図６に示すように、制御対象である倒立振子モデル３０からは、観測量としてｄη／ｄｔと、（θ１−η）と、ｄ（θ１−η）／ｄｔが観測される。観測された観測量には観測ノイズ３２が加えられる。具体的には、観測量ｄη／ｄｔには観測ノイズｎ２が、観測量（θ１−η）には観測ノイズｎ１が、観測量ｄ（θ１−η）／ｄｔには観測ノイズｎ３が加えられる。
観測ノイズ３２が加えられた観測量と目標値３４との偏差は、ロバストコントローラ３６（すなわち、第１制御指令値算出手段２２）に入力する。ここで、観測量ｄη／ｄｔには目標値「０」が、観測量（θ１−η）には目標値「（θ１−η）^＊」が、観測量ｄ（θ１−η）／ｄｔには目標値「ｄ（θ１−η）^＊／ｄｔ」が与えられる。ｄη／ｄｔ（すなわち、ジャイロセンサ１９の出力）に目標値「０」を与えることで、車体の鉛直方向からの傾斜角の角速度は０となり、車体は倒立姿勢を保つこととなる。
ロバストコントローラ３６からはトルク指令値ｕが出力される。そして、その出力されたトルク指令値ｕと外乱ｗが倒立振子モデル３０に入力されることとなる。

ここで、かかる制御系を評価するための評価値としては、例えば、ロバストコントローラ３６からの出力ｕや車輪の回転角θ１を用いることができる。出力ｕを評価するための重み関数Ｗと回転角θ１を評価する評価関数Ｑは、例えば、シミュレーション等によりある程度の絞り込みを行い、実験によって最終的に決定することができる。本実施形態では、下記に示す関数Ｗ，Ｑを用いている。

なお、ロバストコントローラ３６（すなわち、第１制御指令値算出手段２２）の具体的な設計は、公知となっている種々の制御系設計ツールを用いることができる。

次に、第２制御指令値算出手段２６について説明する。第２制御指令値算出手段２６は、台車回転方向を制御するためのトルク指令値を算出する。
ここで、台車回転方向に関しては、移動台車を真上から見て、２輪車としてモデル化する（図５参照）。図５中、φは移動台車の台車回転角を表し、ｄは両車輪１２，１３間の距離を表し、ｒは車輪の半径を表している。なお、図５に示す幾何学的関係から、左右の車輪速度（右車輪の速度ｄθ_Ｒ／ｄｔ，左車輪の速度ｄθ_Ｌ／ｄｔ）を直交座標系での移動台車の位置の速度（ｄｘ／ｄｔ，ｄｙ／ｄｔ）と台車回転方向の角速度（ｄφ／ｄｔ）に変換するためのヤコビ行列は下記に示すようになる。

図７に第２制御指令値算出手段２６による制御系の全体構成を示している。図７から明らかなように、第２制御指令値算出手段２６は、現在位置（ｘ，ｙ，φ）と目標位置（ｘ，ｙ，φ）^＊の偏差に所定のゲイン５０を乗じたものと、現在速度〔（ｄｘ／ｄｔ，ｄｙ／ｄｔ，ｄφ／ｄｔ）〕と目標速度〔（ｄｘ／ｄｔ，ｄｙ／ｄｔ，ｄφ／ｄｔ）^＊〕の偏差に所定のゲイン５２を乗じたものを加算し、その加算した値からモータ１４，１５を制御するためのトルク指令値Ｔ_Ｒ ^＊，Ｔ_Ｌ ^＊を算出している（いわゆる、ＰＤ制御を行っている）。
なお、移動台車は、平面上の位置として２自由度、台車回転方向に１自由度の計３自由度を持つが、アクチュエータとしてはモータ１４，１５の２個しか有さない。このため、上記した位置の偏差（詳しくは、位置の偏差にゲイン５０が乗じられた値）と上記した速度の偏差（詳しくは、速度の偏差にゲイン５２が乗じられた値）とを加算したものに、転置ヤコビ行列Ｊ^Ｔ５４を用いて、直接トルク指令値Ｔ_Ｒ ^＊，Ｔ_Ｌ ^＊を算出している。
また、右車輪１２の回転角速度ｄθ_Ｒ／ｄｔ（すなわち、右車輪速度）と左車輪１３の回転角速度ｄθ_Ｌ／ｄｔ（すなわち、左車輪速度）にヤコビ行列５６（数６に示す行列）をかけることで、現在速度〔（ｄｘ／ｄｔ，ｄｙ／ｄｔ，ｄφ／ｄｔ）〕を算出している。
なお、図７に示す制御系によっても移動台車の平面内での位置（ｘ，ｙ）と、その速度（ｄｘ／ｄｔ，ｄｙ／ｄｔ）について制御することができるが、本実施形態では移動台車の平面内での位置及び速度は第１制御指令値算出手段２２によって制御するため、ゲイン５０，５２において、位置成分（ｘ，ｙ）とその速度成分（ｄｘ／ｄｔ，ｄｙ／ｄｔ）に乗じるゲイン（係数）は「０」としている。したがって、移動台車の台車回転角φの偏差と台車回転角速度ｄφ／ｄｔの偏差のみが、第２制御指令値算出手段２６で使用される。

次に、制御アーム１１を制御する制御系（すなわち、図３に示す制御目標値算出手段２７と第３制御指令値算出手段２９）について説明する。
制御目標値算出手段２７は、制御アーム１１に作用する重力トルクが台車の加減速による加速トルクと釣り合うように制御アーム１１の目標スライド量ｓ^＊を算出する。すなわち、図９に示すように、車体（台車本体１０と制御アーム１１）に作用する重力による重力方向ベクトル（ｍ_１ｇ）と、車体に作用する加速度の反対方向の反加速度方向ベクトル（ｍ_１α）の合成ベクトルが車軸Ｃを通るとき、車体は安定して倒立状態を維持する。ここで、車体の重心高さをｌ、車体の全重量をｍ_１、車体のスライド量ｓ_０とし、また、台車本体１０の傾斜角度ηは小さく、cosη＝１とする。図９に示す幾何学的関係より、次の式が成立する。

このスライド量ｓ_０は車体（台車本体１０と制御アーム１１）の全質量ｍ_１がスライドした場合の移動量である。従って、移動する質量が制御アーム１１（質量ｍ_４）だけの場合は、制御アーム１１をさらに大きく移動させる必要がある。すなわち、制御アーム１１のスライド移動のみによって、車体の全質量ｍ_１がスライド量ｓ_０だけスライドしたときと同様の重力トルクを発生させる必要がある。このための制御アーム１１のスライド量をｓ_ｒとすると、スライド量ｓ_０とスライド量ｓ_ｒとの間には次の関係が成立する（図１０参照）。

したがって、台車が加速度αで移動する際の加速度トルクと釣り合う重力トルクを発生するためには、制御アーム１１のスライド量ｓ_ｒを−（ｍ_１／ｍ_４）・（α／ｇ）・ｌとすればよい。ここで、ｍ_４，ｍ_１，ｇ，ｌは実測等によって特定することができる。従って、台車の加速度αを特定できれば、制御アーム１１のスライド量ｓ_ｒを特定することができる。本実施形態では、台車の加速度αを台車の目標車輪角度θ^＊［θ^＊＝（θ_１−η）^＊（図１、図４参照）］を２回微分したものから求めることとしている。

上述した方法によって、制御アーム１１の目標スライド量ｓ^＊（＝ｓ_ｒ）を算出すると、第３制御指令値算出手段２９はその目標スライド量ｓ^＊を用いてモータ３１のトルク指令値を算出する。具体的には、エンコーダ３１ａによって検出される制御アーム１１の現在位置（スライド量ｓ）と、制御目標値算出手段２７によって算出された制御アーム１１の目標スライド量ｓ^＊の偏差を入力とし、この入力された偏差が「０」となるようにモータ３１のトルク指令値を算出する。

上述した制御アーム１１の制御系の全体構成が図８に示されている。図８に示すように、制御目標値算出手段２７は、車輪角度目標値θ^＊を２回微分して車輪加速度目標値を算出する手段２７ａと、算出された車輪加速度目標値に所定のゲインを乗じて目標スライド量ｓ^＊を算出する「加速度−アーム位置」変換ゲイン２７ｂによって構成されている。第３制御指令値算出手段２９には、制御目標値算出手段２７から出力される目標スライド量ｓ^＊とエンコーダ３１ａの検出値から求まる現在のアームスライド量ｓとの偏差が入力され、第３制御指令値算出手段２９はその偏差にゲイン（−Ｋ_ＰＧ）を乗じた値をモータ３１に出力する。なお、ゲインＫ_ＰＧに（−）が付加されているのは、制御アーム１１は台車の反加速度方向に移動するためである。
ここで、上記の車輪角度目標値θ^＊は、目標値入力手段２４によって入力される台車の目標値から算出することができる。例えば、台車の目標値がＸＹ座標系の座標位置（Ｘ^＊，Ｙ^＊）で与えられた場合、この目標位置（Ｘ^＊，Ｙ^＊）を車輪１２，１３の台車並進方向の車輪角度目標値θ^＊に変換する。すなわち、台車の目標位置（Ｘ^＊，Ｙ^＊）から台車の移動距離を算出し、その移動距離から車輪の回転角（車輪角度目標値θ^＊）を算出する。

また、図８から明らかなように、算出手段２７ａで算出された車輪加速度目標値は加減速度補償手段３３（図３においては図示を省略）に入力される。加減速度補償手段３３は、入力された車輪加減速目標値にゲインＫ_ＡＴを乗じて加減速補償値Ｔ_ＡＴを算出する。算出された加減速補償値Ｔ_ＡＴは、上述した第１制御指令値算出手段２２によって算出されたトルク指令値と第２制御指令値算出手段２６によって算出されたトルク指令値との和に加算され、モータ１４，１５に出力される。台車の加減速度を補償するトルクをモータ１４，１５に出力することで、目標値入力手段２４によって入力される台車の目標値に対し、より遅れの小さい移動動作が可能となる。
なお、上記ゲインＫ_ＡＴは、台車並進方向に関する台車全体の慣性の値（大きさ）に設定される。この値は実測あるいは、設計データ等から得ることができる。

以上説明したように、台車並進方向の制御（車軸と直行方向）では倒立振子制御と位置制御が同時に行われ、台車回転方向（車軸旋回方向）では位置制御のみが行われ、これらの制御は互いに干渉しないものとなっている。このため、モータ１４，１５への最終的な制御指令値は、第１制御指令値算出手段２２で算出された制御指令値と第２制御指令値算出手段２６で算出された制御指令値を足し合わせたものとなっている。
また、制御アーム１１の制御は、これら台車並進方向の制御や台車回転方向の制御とは独立して行われる。すなわち、制御アーム１１の目標スライド量ｓ^＊が車輪１２，１３の車輪角度目標値θ^＊からのみ算出される。したがって、台車の制御が不安定となっても制御アーム１１の制御には影響せず、台車のノイズなどの影響を受けることなく安定した制御を行うことができる。
なお、上述した台車本体１０の制御系と、制御アーム１１の制御系の両者を図１１に併せて示す。

次に、目標値入力手段２４について説明する。図１２には目標値入力手段２４の構成を示すブロック図が示されている。図１２に示すように目標値入力手段２４は、目標軌道データ記憶手段６０と、並進方向目標値算出手段６２と、回転方向目標値算出手段６４で構成されている。

目標軌道データ記憶手段６０は、移動台車の軌道と、軌道上の各位置における移動台車の速度と加速度、並びに、移動台車の台車回転方向の角速度と角加速度を規定する目標軌道データを記憶する。本実施形態では、移動台車の軌道を等加速度運動〔加速度０の場合（等速運動の場合）も等加速度運動として取扱っている〕を行っている区間に分割し、分割された各区間の（区間時間ｔ，区間加速度ａ，区間初速度ｂ，区間角加速度ａ’，区間角初速度ｂ’）が目標軌道データとされる。すなわち、移動台車の並進方向の速度をｖ、移動台車の台車回転方向の速度をｄφ／ｄｔ、制御開始からの経過時間をｔとすると、これらの関係は次に示す式で表される。

図１３には、移動台車を移動させる軌道の一部と、そのときの目標軌道データを示している。図１３から明らかなように、移動台車を移動させる軌道上のＡ点からＢ点までは台車並進方向に等加速度ａ_１で等加速度運動を行い、台車回転方向の速度及び加速度は「０」である。Ｂ点からＣ点までは台車並進方向に等速度運動を行い、台車回転方向に等角加速度ａ_１’で等角加速度運動を行う。Ｃ点からＤ点までは台車並進方向に等速度運動を行い、台車回転方向にも等角速度運動を行う。
したがって、目標軌道データは、Ａ点からＢ点までの運動を規定するデータと、Ｂ点からＣ点までの運動を規定するデータと、Ｃ点からＤ点までの運動を規定するデータにより構成される。すなわち、Ａ点からＢ点までの運動を規定する目標軌道データは（ｔ_１，ａ_１，０，０，０）となり、Ｂ点からＣ点までの運動を規定する目標軌道データは（ｔ_２，０，ａ_１ｔ_１，ａ_１’，０）となり、Ｃ点からＤ点までの運動を規定する目標軌道データ（ｔ_３，０，ａ_１ｔ_１，０，ａ_１’ｔ_２）となる。

並進方向目標値算出手段６２は、目標軌道データ記憶手段６０に記憶されている目標軌道データからＸ−Ｙ平面内における移動台車の目標位置（ｘ、ｙ）^＊と目標速度（ｄｘ／ｄｔ，ｄｙ／ｄｔ）^＊を算出する。例えば、図１３に示すＡ点を原点として運動を開始した場合において運動開始から時間ｔ（ただし、０＜ｔ＜ｔ_１）を経過したときは、目標位置（ｘ、ｙ）^＊＝（ａ_１ｔ^２／２，０）^＊となり、目標速度（ｄｘ／ｄｔ，ｄｙ／ｄｔ）^＊＝（ａ_１ｔ，０）^＊となる。
回転方向目標値算出手段６４は、目標軌道データ記憶手段６０に記憶されている目標軌道データから移動台車の目標回転角（φ）^＊と目標速度（ｄφ／ｄｔ）^＊を算出する。ここで、目標回転角（φ）^＊は移動台車の総回転角量を意味する。したがって、移動台車が同一姿勢（すなわち、同一台車角度）となっている場合でも、回転角量（旋回数）が異なる場合は目標回転角（φ）^＊も異なることとなる。
並進方向目標値算出手段６２と回転方向目標値算出手段６４によって算出された目標位置（ｘ，ｙ，φ）^＊と目標速度（ｄｘ／ｄｔ，ｄｙ／ｄｔ，ｄφ／ｄｔ）^＊は、第１制御指令値算出手段２２と第２制御指令値算出手段２６の目標値として用いられる。すなわち、図６，７に示すように、第１制御指令値算出手段２２（詳しくは、図６に示すロバストコントローラ３６）には、上記の目標位置と目標速度が台車中心位置（θ１−η）と台車中心速度ｄ（θ１−η）／ｄｔに変換されて用いられる。また、第２制御指令値算出手段２６には、上記の目標位置（ｘ，ｙ，φ）^＊が用いられる（ただし、ゲイン５０のうちｘ、ｙに関する係数は０であるため、実際にはφのみが用いられる）。
また、並進方向目標値算出手段６２で算出された目標位置（ｘ，ｙ，φ）^＊は、制御目標値算出手段２７によって目標スライド量ｓ^＊を算出するために用いられる。

次に、上述のように構成される制御コンピュータ２０において行われる処理について説明する。図１４は制御コンピュータ２０の処理手順を示すフローチャートである。
図１４に示すように、制御コンピュータ２０は、まず、各モータ１４，１５，３１のエンコーダ１４ａ，１５ａ，３１ａの値（すなわち、車輪１２，１３の回転角度θ_Ｒ，θ_Ｌと制御アーム１１のスライド量ｓ）を読込む（Ｓ１０）。
次に、ステップＳ１０で読込んだエンコーダ１４ａ，１５ａの値から車輪速度（ｄθ_Ｒ／ｄｔ，ｄθ_Ｌ／ｄｔ）と、現在速度・現在方向速度（ｄｘ／ｄｔ，ｄｙ／ｄｔ，ｄφ／ｄｔ）を算出する（Ｓ１２）。すなわち、エンコーダ１４ａ，１５ａの値の時間的変化量から車輪１２，１３の車輪速度（ｄθ_Ｒ／ｄｔ，ｄθ_Ｌ／ｄｔ）を算出し、これら算出された車輪速度（ｄθ_Ｒ／ｄｔ，ｄθ_Ｌ／ｄｔ）とヤコビ行列５６とから現在速度・現在方向速度（ｄｘ／ｄｔ，ｄｙ／ｄｔ，ｄφ／ｄｔ）を算出する。
ステップＳ１４では、ジャイロセンサ１９の出力値ｄη／ｄｔを読込む。
ステップＳ１６では、制御コンピュータ２０が起動されてからの時間ｔ（すなわち、移動台車の軌道制御開始時からの経過時間）と、目標軌道データ記憶手段６０に記憶されている目標軌道データとから、目標位置（ｘ，ｙ，φ）^＊と目標速度（ｄｘ／ｄｔ，ｄｙ／ｄｔ，ｄφ／ｄｔ）^＊を算出する。また、その算出した目標位置（ｘ，ｙ，φ）^＊から目標車輪角度θ^＊を算出し、その目標車輪角度から制御アーム１１の目標スライド量ｓ^＊を算出する。
ステップＳ１０からステップＳ１６までの処理により移動台車の現在値と目標値が算出されるため、次に、台車並進方向に関するモータ１４，１５のトルク指令値Ｔ_Ｒ１，Ｔ_Ｌ１をそれぞれ算出し（Ｓ１８）、台車回転方向に関するモータ１４，１５のトルク指令値Ｔ_Ｒ２，Ｔ_Ｌ２をそれぞれ算出し（Ｓ２０）、さらに、移動台車の加速度を補償するトルク指令値Ｔ_Ｒ３，Ｔ_Ｌ３を算出する（Ｓ２２）。すなわち、第１制御指令値算出手段２２によってトルク指令値Ｔ_Ｒ１，Ｔ_Ｌ１を算出し、第２制御指令値算出手段２６によってトルク指令値Ｔ_Ｒ２，Ｔ_Ｌ２を算出し、加減速度補償手段３３によってトルク指令値Ｔ_Ｒ３，Ｔ_Ｌ３を算出する。
ステップＳ２４では、ステップＳ１６で算出した制御アーム１１の目標スライド量ｓ^＊とステップＳ１０で読込んだ制御アーム１１の現在のスライド量ｓに基づいて、制御アーム１１を駆動するモータ３１のトルク指令値Ｔ_Ｓを算出する。算出されたトルク指令値Ｔ_Ｓはモータ駆動回路１７に出力される。モータ駆動回路１７は、入力するトルク指令値Ｔ_Ｓに基づいてモータ３１を駆動する。これによって制御アーム１１が所定の目標位置までスライドする。
ステップＳ２６では、ステップＳ１８で算出されたトルク指令値Ｔ_Ｒ１，Ｔ_Ｌ１と、ステップＳ２０で算出されたトルク指令値Ｔ_Ｒ２，Ｔ_Ｌ２と、ステップＳ２２で算出されたトルク指令値Ｔ_Ｒ３，Ｔ_Ｌ３を加算し、これらの値をモータ駆動回路１６ａ，１６ｂに出力する。これによって、各車輪１２，１３が駆動されることとなる。ステップＳ２６が終わるとステップ１０に戻り、次の制御タイミングにおける処理が開始される。
なお、ステップＳ１０〜ステップＳ２６までの処理は、所定の時間間隔（例えば、１０ｍｓ）で行われ、これによって移動台車は目標軌道データで規定された軌道を所定の速度・加速度・角速度・角加速度で運動することとなる。

上述した説明から明らかなように、本実施形態の移動台車ではＨ∞制御理論を用いて倒立振子制御系を設計している。このため、設置面の斜度の変化や負荷の変動等の外乱に対して安定して倒立を維持することができる。また、Ｈ∞制御理論を用いることで、制御対象である移動台車の厳密なモデル化を不要としている。すなわち、Ｈ∞制御理論を用いているため、制御アーム１１が中立位置（スライド量ｓ＝０）で固定されていると仮定してモデル化しても、台車本体１０は安定して倒立を維持することができる。
また、制御アーム１１の目標スライド量ｓ^＊は台車の車輪角度目標値θ^＊のみに基づいて算出される。このため、制御アーム１１の制御系を簡易に構成することができる。また、ゲイン数も少なくなるため、その調整を容易に行うことができる。さらに、制御アーム１１の状態量を用いて目標スライド量ｓ^＊が算出されないため、制御アーム１１は安定して制御することができる。

また、本実施形態の移動台車では、台車の加減速に同期して制御アーム１１が駆動され、台車の加速トルクを制御アーム１１の重力トルクが補償する。このため、加速時や減速時の台車本体１０の傾斜角度を小さくすることができる。このため、台車本体１１上に搬送物を載置して運搬する場合等には、その搬送物を安定して運搬することができる。また、台車の底面が接地面に接触しにくくなるため、走行の安定性を増すことができ、あるいは、台車底面が接地面に接触するまでの余裕度が増すため、設計の自由度を増大することができる。さらに、加速時や減速時の台車本体１０の傾きや遅れが小さくなるため、台車の加速度、減速度を大きくすることができる。従って、台車の最高速度を上げることができ、台車を高速で移動させることができる。
さらに、本実施形態の移動台車では、台車の加速又は減速に必要な加減速補償トルクが車輪１２，１３に加えられる。このため、目標値が出てから実際に台車が動き出すまで、あるいは、停止するまでの時間遅れを小さくすることができる。したがって、発進、停止動作を繰り返し与えても時間遅れが小さいので、軌跡ずれを小さくすることができる。

以上、本発明の好適ないくつかの実施形態について詳細に説明したが、これらは例示に過ぎず、本発明は当業者の知識に基づいて種々の変更、改良を施した形態で実施することができる。
例えば、上述した実施形態では台車本体１０に対して制御アーム１１がスライド移動する構成であったが、本発明はこのような形態に限られない。例えば、図１５に示すように台車本体１００に制御アーム１０２が回転可能に取付けられているものであってもよい。このような構成によっても、制御アーム１０２を回転駆動することで、台車の加速トルクを補償することができる。図１６に示す場合は、制御アーム１０２の目標スライド量ｓ_ｒ（制御アーム１０２の重心位置のスライド量）はｌ_２sinσ_ｒとなり、制御アーム１０２の目標回転角σ_ｒはsin^−１（ｓ_ｒ／ｌ_２）となる。ここで、ｌ_２は制御アーム１０２の基端部（台車本体１００への取付位置）から制御アーム１０２の重心位置までの距離であり、設計データ等から得ることができる。また、ｓ_ｒは上述の実施形態と同様に台車の目標車輪角度から得ることができる。ｌ_２とｓ_ｒが決まれば、これらの値を用いて制御アーム１０２の目標回転角σ_ｒを求めることができる。

また、上述した実施形態は駆動輪を２つ有していたが、本発明に係る技術はこのような例に限られず、例えば、駆動輪を１輪のみとし他の車輪を従輪としてもよい。また、上述した実施形態では、車体の傾斜角速度を測定するセンサにジャイロセンサを用いたが、このようなセンサに限られず、傾斜角や傾斜角速度の計測に用いることができる種々の計測器（例えば、重力加速度センサ、重り吊り下げ型傾斜角度計等）を用いることもできる。さらに、図１４に示す制御フローは、制御コンピュータ２０によって行われる処理の一例であり、これらのステップの順序を変えて種々に実施することもできる。

なお、本明細書または図面に説明した技術要素は、単独であるいは各種の組み合わせによって技術的有用性を発揮するものであり、出願時請求項記載の組み合わせに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

本実施形態に係る移動台車の構成を模式的に示す図。 本実施形態に係る移動台車の台車本体の斜視図。 移動台車の制御系の構成を示す機能ブロック図。 移動台車を並進方向に関してモデル化した図。 移動台車を台車の回転方向に関してモデル化した図。 移動台車の並進方向に関する制御系の構成を示す図。 移動台車の台車回転方向に関する制御系の構成を示す図。 制御アームに関する制御系の構成を示す図。 制御アームの目標スライド量を決定する方法を説明するための図。 制御アームの目標スライド量を決定する手順を説明するための図。 移動台車の並進方向の制御系と台車回転方向の制御系と制御アームの制御系を組合せた状態を示す図。 目標値入力手段の構成を示すブロック図。 移動台車の目標軌道の一例と、その目標軌道を達成するための目標軌道データの一例を併せて示す図。 制御コンピュータにより行われる処理手順の一例を示すフローチャート。 本実施形態に係る移動台車の変形例を模式的に示す図。 図１５に示す移動台車において制御アームの目標回転角を決定する方法を説明するための図。

符号の説明

１０：台車本体
１２：右車輪
１３：左車輪
１４，１５，３１：モータ
１６：モータドライバ
１８：バッテリ
１９：ジャイロセンサ
２０：制御コントローラ

Claims (6)

1. 車軸と、車軸上に配置された２以上の車輪と、車軸に対し回動可能に支持された車体を備える倒立振子型の移動台車であり、
   車体に対して少なくとも台車進行方向に移動可能に取付けられた慣性体と、
   車輪を駆動する第１アクチュエータと、
   慣性体を駆動する第２アクチュエータと、
   車輪の位置、速度及び加速度の少なくとも１つを検出する第１検出手段と、
   車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第２検出手段と、
   車体に対する慣性体の位置、速度及び加速度の少なくとも１つを検出する第３検出手段と、
   車輪の位置、速度及び加速度の少なくとも１つに関して目標値を入力する入力手段と、
   車輪の位置、速度及び加速度の少なくとも１つが入力された目標値となり、かつ、車体が倒立状態を維持するように、入力された目標値と第１及び第２検出手段により検出された検出値とに基づいて、第１アクチュエータの制御指令値を算出する第１コントローラと、
   入力された車輪の目標値のみに基づいて、車体に対する慣性体の位置、速度及び加速度の少なくとも１つに関して目標値を算出する手段と、
   算出された目標値と第３検出手段により検出された検出値とに基づいて第２アクチュエータの制御指令値を算出する第２コントローラと、を有する移動台車。
2. 前記目標値算出手段は、前記入力手段によって入力された目標値から決定される車輪の目標加速度に基づく加速トルクが慣性体の重力トルクと釣り合うように慣性体の目標位置を算出することを特徴とする請求項１に記載の移動台車。
3. 前記第１アクチュエータは、（１）第１コントローラによって算出される制御指令値と、（２）前記入力手段によって入力された目標値から決定される車輪の加減速度を補償する加減速補償値との和に基づいて駆動されることを特徴とする請求項１又は２に記載の移動台車。
4. 前記第１コントローラは、車体に対して慣性体が所定の位置で固定されていると仮定して導出した運動方程式に基づいて設計されていることを特徴とする請求項１〜３のいずれかに記載の移動台車。
5. 前記第１コントローラは、外乱に対して安定倒立可能なようにロバスト性を有することを特徴とする請求項４に記載の移動台車。
6. 車軸と、車軸上に配置された２以上の車輪と、車軸に対し回動可能に支持された車体と、車体に対して少なくとも台車進行方向に移動可能に取付けられた慣性体と、車輪を駆動する第１アクチュエータと、慣性体を駆動する第２アクチュエータと、第１アクチュエータと第２アクチュエータに制御指令値を出力する制御コンピュータと、車輪の位置、速度及び加速度の少なくとも１つを検出する第１検出手段と、車体の傾斜角度及び傾斜角速度の少なくとも１つを検出する第２検出手段と、車体に対する慣性体の位置、速度及び加速度の少なくとも１つを検出する第３検出手段とを備える倒立振子型の移動台車の制御方法であり、
   第１、第２及び第３検出手段で検出される検出値を読込む工程と、
   車輪の位置、速度及び加速度の少なくとも１つに関して目標値を読込む工程と、
   車輪の位置、速度及び加速度の少なくとも１つが読込んだ目標値となり、かつ、車体が倒立状態を維持するように、読み込んだ目標値と、第１及び第２検出手段より読込んだ検出値とに基づいて、第１アクチュエータの制御指令値を算出する工程と、
   読込んだ目標値のみに基づいて、車体に対する慣性体の位置、速度及び加速度の少なくとも１つに関して目標値を算出する工程と、
   算出された目標値と第３検出手段より読込んだ検出値とに基づいて第２アクチュエータの制御指令値を算出する工程と、
   算出された各制御指令値を第１及び第２アクチュエータのそれぞれに出力する工程と、を有し、
   上記各工程を制御コンピュータで繰返し実行することで移動台車を制御することを特徴とする移動台車の制御方法。

Images