JP2004291799A - Movable carriage - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To hold an inverted posture of a carriage body of an inverted pendulum type movable carriage when the control for the inversion has been stopped. <P>SOLUTION: The movable carriage is configured as follows. Auxiliary legs 21, 22 are attached to a mounting portion 1e of the carriage body 1 so as to rotate about an axis in parallel to an axle, and castor wheels 21d, 22d provided at the tip end of the auxiliary legs 21, 22 can come into contact with the ground apart from the grounding point of a main wheel 3 nearly in the travelling direction. The arrangement of the caster wheels 21d, 22d is changed to the position to be grounded to the road surface and the position separated from the road surface by turning the auxiliary legs 21, 22. <P>COPYRIGHT: (C)2005,JPO&NCIPI

Description

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

【００２１】
したがって、上記の式から次の状態方程式が導出される。
【００２２】
【数３】

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

【００２５】
上述した手順でモデル化された制御系の全体構成を図１０に示す。図１０に示すように、制御対象である倒立振子モデル４１からは、観測量としてｄη／ｄｔと、（θ１−η）と、ｄ（θ１−η）／ｄｔとが観測される。観測された観測量には観測ノイズ４２が加えられる。具体的には、観測量ｄη／ｄｔには観測ノイズｎ２が、観測量（θ１−η）には観測ノイズｎ１が、観測量ｄ（θ１−η）／ｄｔには観測ノイズｎ３が加えられる。
観測ノイズ４２が加えられた観測量と目標値４３との偏差は、ロバストコントローラ４４（すなわち、第１制御指令値算出手段２６）に入力する。ここで、観測量ｄη／ｄｔには目標値「０」が、観測量（θ１−η）には目標値「（θ１−η）^＊」が、観測量ｄ（θ１−η）／ｄｔには目標値「ｄ（θ１−η）^＊／ｄｔ」が与えられる。ｄη／ｄｔ（すなわち、ジャイロセンサ１９の出力）に目標値「０」が与えられるため（すなわち、車体の鉛直方向からの傾き角速度は０）、車体は倒立姿勢を保つこととなる。
ロバストコントローラ４４からはトルク指令値ｕが出力される。そして、出力されたトルク指令値ｕと外乱ｗが倒立振子モデル４１に入力されることとなる。
【００２６】
ここで、かかる制御系を評価するための評価値としては、例えば、ロバストコントローラ３６からの出力ｕや車輪の回転角θ１を用いることができる。出力ｕを評価するための重み関数Ｗ４６と回転角θ１を評価する評価関数Ｑ４５は、例えば、シミュレーション等によりある程度の絞り込みを行い、実験によって最終的に決定することができる。本実施形態では、下記に示す関数を用いている。
【００２７】
【数５】

【００２８】
なお、ロバストコントローラ４４の具体的な設計には、公知となっている種々の制御系設計ツールを用いることができる。
【００２９】
次に、第２制御指令値算出手段２８について説明する。第２制御指令値算出手段２８は、台車回転方向に関する台車の運動を制御するためのトルク指令値を算出する。
ここで、台車回転方向に関しては、移動台車を真上から見て、２輪車としてモデル化している（図９参照）。図９中、φは移動台車の台車回転角を表し、ｄは両車輪３，４間の距離を表し、ｒは車輪の半径を表している。なお、図９に示す幾何学的関係から、左右の車輪速度（右車輪の速度ｄθ_Ｌ／ｄｔ，左車輪の速度ｄθ_Ｒ／ｄｔ）を直交座標系での移動台車の位置の速度（ｄｘ／ｄｔ，ｄｙ／ｄｔ）と台車回転方向の角速度（ｄφ／ｄｔ）に変換するためのヤコビ行列は下記に示すようになる。
【００３０】
【数６】

【００３１】
図１１に第２制御指令値算出手段２８による制御系の全体構成を示している。図１１から明らかなように、第２制御指令値算出手段２８は、現在位置（ｘ，ｙ，φ）と目標位置（ｘ，ｙ，φ）^＊の偏差に所定のゲイン５０を乗じたものと、現在速度〔（ｄｘ／ｄｔ，ｄｙ／ｄｔ，ｄφ／ｄｔ）〕と目標速度〔（ｄｘ／ｄｔ，ｄｙ／ｄｔ，ｄφ／ｄｔ）^＊〕の偏差に所定のゲイン５２を乗じたものを加算し、その加算した値からモータ５，６を制御するためのトルク指令値Ｔ_Ｒ ^＊，Ｔ_Ｌ ^＊を算出している（いわゆる、ＰＤ制御を行っている）。
なお、移動台車は、平面上の位置として２自由度、台車回転方向に１自由度の計３自由度を持つが、アクチュエータとしてはモータ１４，１５の計２個しか有さない。このため、上記した位置の偏差にゲイン５０を乗じた値と上記した速度の偏差にゲイン５２を乗じた値とを加算た値と転置ヤコビ行列Ｊ^Ｔ５４を用いて、直接トルク指令値Ｔ_Ｒ ^＊，Ｔ_Ｌ ^＊を算出している。
また、右車輪４の回転角速度ｄθ_Ｒ／ｄｔ（すなわち、右車輪速度）と左車輪３の回転角速度ｄθ_Ｒ／ｄｔ（すなわち、左車輪速度）にヤコビ行列５６（数６に示す行列）をかけることで、現在速度〔（ｄｘ／ｄｔ，ｄｙ／ｄｔ，ｄφ／ｄｔ）〕を算出している。
なお、図１１に示す制御系によっても移動台車の平面内での位置（ｘ，ｙ）とその速度（ｄｘ／ｄｔ，ｄｙ／ｄｔ）について制御することが可能である。しかしながら、これらについては第１制御指令値算出手段２６によって制御するため、ゲイン５０，５２においては、位置成分（ｘ，ｙ）とその速度成分（ｄｘ／ｄｔ，ｄｙ／ｄｔ）に乗じるゲイン（係数）を「０」としている。したがって、移動台車の台車回転角φの偏差と台車回転角速度ｄφ／ｄｔの偏差のみが、第２制御指令値算出手段２８で用いられる。
【００３２】
以上説明したように、台車並進方向の制御（車軸と直行方向）には倒立振子制御と位置制御が同時に行われ、台車回転方向（車軸旋回方向）には位置制御（台車回転角φ）のみが行われ、これらの制御は互いに干渉しないものとなっている。そして、モータ５，６への最終的な制御指令値は、第１制御指令値算出手段２６で算出された制御指令値と第２制御指令値算出手段２８で算出された制御指令値を足し合わせたものとなる。すなわち、制御指令値加算手段２９は、第１制御指令値算出手段２６で算出された制御指令値と第２制御指令値算出手段２８で算出された制御指令値とを加算し、加算した値をモータドライバ１１，１１にそれぞれ出力する。
上述した第１制御指令値算出手段２６と、第２制御指令値算出手段２８と、制御指令値加算手段２９とによって構成される制御系の全体構成を図１２に示している。図１２から明らかなように、本実施形態の制御系は、台車回転方向の制御ループ内に台車並進方向の制御が組み込まれたものとなっている。
【００３３】
次に、目標値入力手段２７について説明する。図１３には目標値入力手段２７の構成を示すブロック図が示されている。図１３に示すように目標値入力手段２７は、目標軌道データ記憶手段６０と、並進方向目標値算出手段６２と、回転方向目標値算出手段６４で構成される。
【００３４】
目標軌道データ記憶手段６０は、移動台車の軌道と、軌道上の各位置における移動台車の速度と加速度、並びに、移動台車の台車回転方向の角速度と角加速度を規定する目標軌道データを記憶する。本実施形態では、移動台車の軌道を等加速度運動〔ただし、加速度０の場合（等速運動の場合）も等加速度運動とみなしている〕を行っている区間に分割し、分割された各区間の（区間時間ｔ，区間加速度ａ，区間初速度ｂ，区間角加速度ａ’，区間角初速度ｂ’）が目標軌道データとされる。すなわち、移動台車の並進方向の速度をｖ、移動台車の台車回転方向の角速度をｄφ／ｄｔ、区間開始からの経過時間をｔとすると、これらの関係は次に示す式で表される。
【００３５】
【数７】

【００３６】
図１４には、移動台車の軌道の一例と、そのときの目標軌道データを示している。図１４に示される軌道では、Ａ点からＢ点までは台車並進方向に加速度ａ_１で等加速度運動を行い、台車回転方向の速度及び加速度は「０」である。Ｂ点からＣ点までは台車並進方向に等速度運動を行い、台車回転方向に角加速度ａ_１’で等角加速度運動を行う。Ｃ点からＤ点までは台車並進方向に等速度運動を行い、台車回転方向にも等角速度運動を行う。
したがって、上述の場合の目標軌道データは、Ａ点からＢ点までの運動を規定するデータと、Ｂ点からＣ点までの運動を規定するデータと、Ｃ点からＤ点までの運動を規定するデータにより構成される。すなわち、Ａ点からＢ点までの運動を規定する目標軌道データは（ｔ_１，ａ_１，０，０，０）となり、Ｂ点からＣ点までの運動を規定する目標軌道データは（ｔ_２，０，ａ_１ｔ_１，ａ_１’，０）となり、Ｃ点からＤ点までの運動を規定する目標軌道データ（ｔ_３，０，ａ_１ｔ_１，０，ａ_１’ｔ_２）となる。
【００３７】
並進方向目標値算出手段６２は、目標軌道データ記憶手段６０に記憶されている目標軌道データからＸ−Ｙ平面内における移動台車の目標位置（ｘ、ｙ）^＊と目標速度（ｄｘ／ｄｔ，ｄｙ／ｄｔ）^＊を算出する。例えば、図１４に示すＡ点を原点（０，０）として運動を開始した場合において運動開始から時間ｔ（ただし、０＜ｔ＜ｔ_１）を経過したときは、目標位置（ｘ、ｙ）^＊＝（ａ_１ｔ^２／２，０）^＊となり、目標速度（ｄｘ／ｄｔ，ｄｙ／ｄｔ）^＊＝（ａ_１ｔ，０）^＊となる。
回転方向目標値算出手段６４は、目標軌道データ記憶手段６０に記憶されている目標軌道データから移動台車の目標回転角（φ）^＊と目標速度（ｄφ／ｄｔ）^＊を算出する。ここで、目標回転角（φ）^＊は移動台車の総回転角量を意味する。したがって、移動台車が同一姿勢（すなわち、台車進行方向からの傾きは同一）となっている場合でも、回転角量（旋回数）が異なる場合は目標回転角（φ）^＊も異なることとなる。
並進方向目標値算出手段６２と回転方向目標値算出手段６４によって算出された目標位置（ｘ，ｙ，φ）^＊と目標速度（ｄｘ／ｄｔ，ｄｙ／ｄｔ，ｄφ／ｄｔ）^＊は、第１制御指令値算出手段２６と第２制御指令値算出手段２８の目標値として用いられる。すなわち、図１２に示すように、第１制御指令値算出手段２６（図１２に示すロバストコントローラ４１）には、上記の目標位置と目標速度が台車中心位置（θ１−η）と台車中心速度ｄ（θ１−η）／ｄｔに変換されて用いられる。また、第２制御指令値算出手段２８には、上記の目標位置（ｘ，ｙ，φ）^＊が用いられる（ただし、ゲイン５０のうちｘ、ｙに関する係数は０であるため、実際にはφ^＊のみが用いられる）。
【００３８】
次に、上述のように構成される移動台車を初期位置から最終目標位置に移動させる際に、制御コンピュータ１２によって行われる処理について図１５を参照して説明する。図１５は制御コンピュータ１２の処理手順を示すフローチャートである。なお、移動台車を初期位置から最終目標位置に移動させるための軌道は、上述した目標軌道データによって与えられる。
制御プログラムが起動されると、制御コンピュータ１２は、まず、切替えスイッチ７３に向って出力している作動信号をＯＦＦからＯＮにする（Ｓ１０）。これによって切替えスイッチ７３がＯＮとなり、アクチュエータ２５のロッドが収縮するように作動する。このため、路面に接地していたキャスター輪２１ｄ，２２ｄが持ち上げられ、路面から離れた位置に上昇する（図１，図２参照）。この状態では主車輪３，４のみが接地しており、この主車輪３，４を制御装置によって回転制御することによって台車本体１が倒立姿勢を保つことができ、また、小回りを利かした走行をすることができる。また、キャスター輪２１ｄ，２２ｄが持ち上げられているため、移動台車は路面に段差等があっても容易に走破することができる。なお、図５のテーパ部材２２ｃを予め抜き差しすることによって、キャスター輪２１ｄ，２２ｄの高さ位置を使用環境（路面の凹凸状況）に適したものに設定しておくことが好ましい。
【００３９】
次いで、制御コンピュータ１２は移動台車が最終目標位置に到達したか否かを判断する（Ｓ１２）。具体的には、目標軌道データ記憶手段６０に記憶されている目標軌道データの最後のデータまで処理したか否かで判断する。
移動台車が最終目標位置に到達している場合〔ステップＳ１２でＹＥＳ〕は、ステップＳ２８に進んで、制御コンピュータ１２は切替えスイッチ７３に向って出力している作動信号をＯＮからＯＦＦにし（Ｓ２８）、その処理を終了する。これによって、切替えスイッチ７３がＯＦＦとなり、アクチュエータ２５のロッドが伸長するように作動する。このため、路面から離れた位置に配置されていたキャスター輪２１ｄ，２２ｄが路面まで降下する（図３，図４参照）。この状態では、キャスター輪２１ｄ，２２ｄは主車輪３，４の接地点の前後の位置で路面に接地し、台車本体１が大きく傾くことなく倒立姿勢のまま静止される。
なお、補助脚２１，２２を直接路面に接地しても転倒防止にはなるが、本実施形態ではキャスター輪を備えることで、その後の移動が容易となるため点検作業等に便利である。
【００４０】
逆に、移動台車が最終目標位置に到達していない場合〔ステップＳ１２でＮＯ〕は、ステップＳ１４に進んで、モータ５，６のエンコーダ５ｃ，６ｃの値（すなわち、車輪３，４の回転角度θ_Ｒ，θ_Ｌ）を読込む（Ｓ１４）。
次に、ステップＳ１４で読込んだエンコーダ５ｃ，６ｃの値から車輪速度（ｄθ_Ｒ／ｄｔ，ｄθ_Ｌ／ｄｔ）と、現在速度・現在方向速度（ｄｘ／ｄｔ，ｄｙ／ｄｔ，ｄφ／ｄｔ）を算出する（Ｓ１６）。すなわち、エンコーダ５ｃ，６ｃの値の時間的変化量から車輪３，４の車輪速度（ｄθ_Ｒ／ｄｔ，ｄθ_Ｌ／ｄｔ）を算出し、これら算出された車輪速度（ｄθ_Ｒ／ｄｔ，ｄθ_Ｌ／ｄｔ）とヤコビ行列５６から現在速度・現在方向速度（ｄｘ／ｄｔ，ｄｙ／ｄｔ，ｄφ／ｄｔ）を算出する（図１２参照）。
ステップＳ１８では、ジャイロセンサ１９の出力値ｄη／ｄｔを読込む。
ステップＳ２０では、制御コンピュータ１２が起動されてからの時間ｔ（すなわち、移動台車の軌道制御開始時からの経過時間）と、目標軌道データ記憶手段６０に記憶されている目標軌道データとから、目標位置（ｘ，ｙ，φ）^＊と目標速度（ｄｘ／ｄｔ，ｄｙ／ｄｔ，ｄφ／ｄｔ）^＊を算出する。
ステップＳ１４からステップＳ２０までの処理により移動台車の現在値と目標値が算出されるため、次に、台車並進方向に関するモータ５，６のトルク指令値Ｔ_Ｒ１，Ｔ_Ｌ１をそれぞれ算出し（Ｓ２２）、台車回転方向に関するモータ５，６のトルク指令値Ｔ_Ｒ２，Ｔ_Ｌ２をそれぞれ算出する（Ｓ２４）。すなわち、第１制御指令値算出手段２６によってトルク指令値Ｔ_Ｒ１，Ｔ_Ｌ１を算出し、第２制御指令値算出手段２８によってトルク指令値Ｔ_Ｒ２，Ｔ_Ｌ２を算出する。
ステップＳ２６では、ステップＳ２２で算出されたトルク指令値Ｔ_Ｒ１，Ｔ_Ｌ１と、ステップＳ２４で算出されたトルク指令値Ｔ_Ｒ２，Ｔ_Ｌ２を加算し、これらの値を対応するモータドライバ１１，１１に出力する。これによって、各車輪３，４が駆動されることとなる。ステップＳ２６が終わるとステップ１２に戻り、次の制御タイミングにおける処理が開始される。
なお、ステップＳ１２〜ステップＳ２６までの処理は、所定の時間間隔（例えば、０．５ｍｓ）で行われ、これによって移動台車は倒立を維持しながら目標軌道データで規定された軌道を所定の速度・加速度・角速度・角加速度で運動する。
【００４１】
なお、上述したステップＳ１２〜ステップＳ２６までの処理が行われているときに非常停止ボタン７２又はリモコンスイッチ７４が操作されると、既に述べたように、制御コンピュータ１２の処理が緊急停止され、かつ、切替えスイッチ７３がＯＦＦされる。この際、制御コンピュータ１２の処理が停止して倒立制御も停止されることとなるが、切替えスイッチ７３がＯＦＦされることでキャスター輪２１ｄ，２２ｄが路面に接地する位置に配置される。このため、台車本体１が転倒することなく倒立姿勢を維持することができる。
【００４２】
上述した説明から明らかなように、本実施形態の移動台車では、制御コンピュータ１２に制御されて移動台車が通常に走行しているときは、キャスター輪２１ｄ，２２ｄが路面から持ち上げられて路面と接触しない。このため、路面が傾斜していたり段差があっても走破することができる。
一方、制御コンピュータ１２の処理が終了し倒立制御を停止するときは、自動的にキャスター輪２１ｄ，２２ｄが路面に接地する位置に配置され、台車本体１の倒立を維持する。したがって、従来必要とされた作業者による転倒防止作業を不要にすることができる。
さらに、非常停止ボタン７２やリモコンスイッチ７４を操作して制御コンピュータ１２の処理を緊急停止させた場合でも、キャスター輪２１ｄ，２２ｄが路面に接地する位置に自動的に配置され、台車本体１の倒立を維持する。したがって、台車本体１の転倒を未然に防止することができ、移動台車の破損を防止することができる。
【００４３】
なお、本発明は上述した実施の形態になんら限定されるものではなく、当業者の知識に基づいて種々の変更、改良を施した形態で実施することができる。
例えば、移動台車の転倒を防止する安全装置の駆動機構は種々の形態をとることができる。安全装置の駆動機構の他の例について図１６及び図１７を参照して説明する。この例は前述のリンク機構に代えてプッシャーを採用したものである。左右対称構成であるから図示を含めて片方のみ説明する。図１６はキャスター輪を下ろした状態の駆動機構を説明する図であり、（イ）は一部破断正面図、（ロ）は一部破断側面図である。図１７はキャスター輪を持ち上げた状態の駆動機構を説明する図であり、（イ）一部破断正面図、（ロ）は一部破断側面図である。
図に示すように、台車本体１の取付部１ｅに設けられた支軸１ｆには、補助脚３１，３２が回動自在に支持されている。補助脚３１の先端にはキャスター輪３１ｄが取り付けられている。
シャーシ１ａには、プッシャー３５とピン引込装置３６が取り付けられている。図中の３７は、プッシャー３５をシャーシ１ａに取付けるためのフランジである。プッシャー３５は内蔵ばね３５ａを備え、内蔵ばね３５ａによってロッド３５ｂが突出方向に付勢されている。ロッド３５ｂの先端にはローラ３５ｃが取付けられている。ローラ３５ｃが補助脚３１の中程に当接した状態でロッド３５ｂが突出することで、補助脚３１が略水平方向に開かれる。このため、図１６に示すように、キャスター輪３１ｄが路面に接地可能な所定位置に配置切り替えされる。
ピン引込装置３６は、例えば、略水平方向に進退可能な可動部を有するリニアアクチュエータであり、その可動部からアーム３６ａが延びている。アーム３６ａの先端には係止部材としての支持ピン３６ｃが形成されている。一方、補助脚３１には支持ピン３６ｃを挿通可能な係止穴３１ａが略水平方向に穿設されている。なお、図１６の位置では支持ピン３６ｃと係止穴３１ａは係合していない。
上記構成の安全装置を有する移動台車を運転するときには、予め、内蔵ばね３５ａのばね力に抗して補助脚３１を手で持ち上げて、キャスター輪３１ｄを路面から離して上方に退避させる。その状態で、図１７に示すように、アーム３６ａの先端の支持ピン３６ｃを係止穴３１ａに差し込んで、補助脚３１を所定の高さ位置に保持せしめる。
その後、移動台車を停止するときには、キャスター輪３１ｄが持ち上げられた図１７の状態からピン引込装置３６を作動させて支持ピン３６ｃを係止穴３１ａから抜き取る方向に移動させる。これによって、補助脚３１はプッシャー３５のばね力によって図７に示す位置まで回動し、キャスター輪３１ｄが路面に接地する。このため、台車本体１は倒立姿勢を維持することができる。
このような機構を用いると、ピン引込装置３６をＯＮ−ＯＦＦ的に動作するだけなので、簡易で安価に駆動機構を構成することができる。
なお、上記のプッシャーはスプリング単体であっても構わない。さらには、キャスター輪を路面に接地する位置と路面から離れた位置とでそれぞれ保持できる保持手段を設けてもよい。
【００４４】
さらに、他の駆動機構の例を図１８，１９を参照して説明する。図１８はキャスター輪を下ろした状態を示す移動台車の側面図であり、図１９は図１８の移動台車の平面図である。
この例は、前述のリンク機構やプッシャーに代えてエアーシリンダを採用している。図１８、１９に示すように、台車本体１の底面には４つのエアーシリンダ３７，３８，３９，４０が取付けられている。４つのエアシリンダー３７，３８，３９，４０は全て同一構成であるので、ここではエアーシリンダ３７の構成についてのみ説明する。図１８に示すように、エアーシリンダ３７はロッド３７ａを有し、ロッド３７ａはシリンダ本体に対して進退動可能となっている。ロッド３７ａの先端にはキャスター輪３７ｂが取付けられている。ロッド３７ａが伸長した状態ではキャスター輪３７ｂが路面と接地する位置に配置され、ロッド３７ａが収縮した状態ではキャスター輪３７ｂが路面から離れた位置に配置されるようになっている。
これら４つのエアーシリンダ３７，３８，３９，４０は、図１９に良く示されるように、主車輪３，４の前方と後方にそれぞれ２つずつ左右に間隔を空けて配設されている。すなわち、主車輪３と４の接地点を結ぶ線分の中点を中心として前後左右の合計４箇所にエアーシリンダ３７，３８，３９，４０が配備されている。
上記構成の安全装置を有する移動台車を運転するときには、予め、エアーシリンダ３７，３８，３９，４０を駆動して各ロッドを収縮させ、キャスター輪を路面から離して上方に退避させる。逆に、移動台車を停止するときには、エアーシリンダ３７，３８，３９，４０を駆動して各ロッドを伸長させ、キャスター輪を路面に接地させる。これによって、台車本体１は倒立姿勢を維持することができる。
このようにエアーシリンダ等によってキャスター輪を直接上下動させる機構を採用すると、緊急停止時においても速やかにキャスター輪を路面に接地させることができる。また、エアーシリンダのロッドを上下動させるだけで良いため、その設置スペースを少なくすることができる。さらに、台車本体の車重を支えるロッドには、圧縮方向の力のみが作用するため、ロッド径を小さくすることができる。
【００４５】
なお、本明細書または図面に説明した技術要素は、単独であるいは各種の組み合わせによって技術的有用性を発揮するものであり、出願時請求項記載の組み合わせに限定されるものではない。また、本明細書または図面に例示した技術は複数の目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
【図面の簡単な説明】
【図１】本実施形態に係る移動台車の正面図。
【図２】同、移動台車の側面図。
【図３】同、移動台車の停止状態を説明する正面図。
【図４】同、移動台車の停止状態を説明する側面図。
【図５】同、安全装置を説明する拡大図。
【図６】同、安全装置を説明する拡大図。
【図７】本実施形態の移動台車の制御系の構成を示す機能ブロック図。
【図８】移動台車を並進方向に関してモデル化した図。
【図９】移動台車を台車の回転方向に関してモデル化した図。
【図１０】本実施形態の移動台車の並進方向に関する制御系の構成を示す図。
【図１１】本実施形態の移動台車の台車回転方向に関する制御系の構成を示す図。
【図１２】本実施形態の移動台車の並進方向の制御系と台車回転方向の制御系とを組合せたときの制御系の構成を示す図。
【図１３】目標値入力手段の構成を示すブロック図。
【図１４】移動台車の目標軌道の一例と、その目標軌道を達成するための目標軌道データの一例を併せて示す図。
【図１５】本実施形態の制御コンピュータにより行われる処理手順を示すフローチャート。
【図１６】安全装置の駆動機構の他の例を説明する図。
【図１７】安全装置の駆動機構の他の例を説明する図。
【図１８】安全装置の駆動機構のさらに他の例を説明する図。
【図１９】安全装置の駆動機構のさらに他の例を説明する図。
【符号の説明】
１：台車本体
３，４：主車輪
５，６：モータ
２０：安全装置
２１，２２：補助脚（支え部材）
２１ｄ，２２ｄ：キャスター輪（支え部材）
２５：アクチュエータ
２３ａ，２４ａ：リンク[0001]
BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a mobile trolley, and more particularly, to a safety device for maintaining an inverted pendulum type mobile trolley in which a center of gravity of a main body of the bogie is located above a rotation axis of wheels, in an inverted posture.
[0002]
2. Description of the Related Art As an inverted pendulum type movable cart, for example, one described in Patent Document 1 is known.
The moving trolley disclosed in Patent Literature 1 includes a pair of wheels, an axle provided between the wheels, and a trolley main body rotatably supported on the axle. The inclination of the bogie body is detected by angle detecting means. This coaxial two-wheeled vehicle has a wheel drive motor for driving wheels, and the wheel drive motor is driven based on a control command value (control torque value) output from a control computer. In the control computer, a control input calculation formula for calculating a control torque value using the lean angle of the vehicle body as an input value and a feedback gain (K) as a coefficient is set.
In such a configuration, the inclination of the vehicle body is sampled at short time intervals by the angle detecting means. Next, the sampled inclination angle is substituted into the control input calculation formula, and the control torque value of the wheel driving motor is calculated. Then, a current command value corresponding to the calculated control torque value is output to the wheel driving motor. Thus, the wheel driving motor is driven in accordance with the inclination of the bogie main body, and the inverting of the bogie main body is maintained.
[0003]
[Patent Document 1]
JP-A-63-305082
[Patent Document 2]
JP-A-02-190277
[0004]
The above-described movable trolley maintains the inversion while the wheel drive control (ie, the inversion control) is being performed, but loses its balance when the inversion control is stopped and falls. Therefore, when the inversion control is stopped, it is necessary for an operator to support the robot and for another operator to perform an operation for preventing a fall by setting an auxiliary stand.
[0005]
The present invention has been made in view of the above-described circumstances, and an object of the present invention is to reduce a burden on a worker to prevent a fall and to easily maintain a movable cart in an inverted posture when the inverted control is stopped. It is to provide technology.
[0006]
In order to solve the above-mentioned problems, a moving trolley according to the present invention comprises wheels and a trolley main body supported by the wheels, and a center of gravity of the trolley main body. Is a movable trolley located above the rotation axis of the wheels, a driving means for driving the wheels, a control means for driving the driving means so that the trolley main body is maintained in an inverted state, and a driving means attached to the trolley main body and A support member that is switchable between a first position that is substantially separated from the point in the traveling direction and touches the road surface and a second position that is separated from the road surface.
In this movable trolley, when the trolley body is controlled to be inverted, the support member can be arranged at the second position away from the road surface. For this reason, when the movable trolley is controlled to be inverted and the drive wheels rotate, it is possible to prevent the support member from hindering. On the other hand, when the bogie main body is not controlled to be inverted, the support member can be arranged at the first position where it comes into contact with the road surface. When the support member is disposed at the first position, the support member comes into contact with the road surface at a position separated from the ground point of the wheel in a substantially traveling direction of the wheel, so that the bogie main body can be prevented from falling down. Since this support member is attached to the bogie main body, it can be easily and quickly operated, and the bogie main body can be maintained in an inverted posture.
In order to more reliably support the bogie main body, it is preferable to provide support members on both the forward and backward sides of the wheels.
Further, in order to stably prevent the bogie main body from falling, when the support member is at the first position, the vertical projection point of the center of gravity of the bogie main body on the road surface (ground surface) is in contact with the support member. It is preferable that the point and the wheel ground point be within a convex polygon having at least three ground points selected so as to increase the distance from the center of gravity projection point.
[0007]
It is preferable that the mobile trolley further includes a drive mechanism that switches the position of the support member between a first position and a second position.
In this configuration, since the arrangement of the support members can be switched by the drive mechanism, it is possible to more easily prevent the fall when the inversion control is stopped.
Note that a drive device such as a motor or an actuator can be used as the drive mechanism, and the drive mechanism may be operated by remote control such as wirelessly.
[0008]
Further, the drive mechanism arranges the support member at the second position when the control means performs the inversion control of the bogie main body, and supports the support member when the control means does not perform the inversion control of the bogie main body. Preferably, the member is located at the first position.
In this configuration, when the inversion control is performed, the support member is automatically arranged at the second position, and when the inversion control is stopped, the support member is automatically arranged at the first position. Since the drive mechanism operates in conjunction with the inversion control of the movable cart, the movable cart can be effectively prevented from falling.
The inversion control of the bogie main body may be automatically stopped by a program installed in advance, or may be stopped by a switch provided separately. The switch for stopping the inversion control may be provided on the bogie main body or may be provided at a position distant from the bogie main body (for example, a remote control switch for outputting a signal wirelessly or the like).
[0009]
Preferably, the support member has a caster wheel.
With this configuration, the mobile trolley can be made to stand alone in the inverted state when the inverted control is stopped, and can be moved in an appropriate direction while maintaining its posture. Therefore, it is easy to handle and the maintainability is improved.
Further, if the bogie is suddenly stopped in an emergency stop or the like and the support member is grounded, the bogie may continue to move for a while due to inertial force. If the caster wheels are grounded, such a movement can be smoothly stopped without hindering such movement, and from this point the overturn prevention effect is further enhanced.
[0010]
DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS One embodiment of the present invention will be described with reference to the drawings. First, a mechanical configuration of the movable trolley will be described with reference to FIGS. Here, FIG. 1 is a front view of an inverted pendulum type coaxial two-wheel mobile trolley, and FIG. 2 is a partially cutaway side view of the same mobile trolley. In both figures, the movable trolley is shown with the auxiliary wheels (caster wheels) raised. FIG. 3 is a front view of the movable trolley when the auxiliary wheels are lowered to a ground state, and FIG. 4 is a side view of the movable trolley.
[0011]
1 to 4, reference numeral 1 denotes a bogie main body, in which a middle storage section 1b is provided in a middle section of a chassis 1a formed of a steel material or the like, and an upper storage section 1c is provided in an upper section. At the lower side of the chassis 1a, that is, at the lowermost part of the bogie main body 1, an attachment portion 1e for attaching a safety device 20 described later is formed. The chassis 1a has bearings 2a and 2b on both left and right sides, and the left and right main wheels 3 and 4 are rotatably supported by the bearings 2a and 2b. 3a and 4a in the figure indicate axles of the main wheels 3 and 4. The two main wheels 3 and 4 are symmetrically disposed at positions sharing the axis of rotation. The chassis 1a is provided with electric motors 5, 6 for individually driving both wheels 3, 4, and speed reducers 5a, 6a for reducing the rotation of the motor. To explain the reduction gear of the present example with respect to the main wheel 3, the pulley 3b provided on the axle 3a side and the pulley 5b attached to the output shaft end of the reduction gear 5a are connected by a belt 7 (FIG. 2). Similarly, the main wheel 4 is configured such that the pulley 4b and the pulley 6b are connected by a belt (not shown). Note that a gear device or the like can be used as the reduction gear.
[0012]
The bogie main body 1 is provided with a control device for controlling the rotation of the main wheels 3 and 4. The control device includes two motor drivers 11, 11 for individually driving the motors, a control computer 12 for controlling the motor drivers 11, 11, a battery 13 as a power supply, and a DC / DC converter 14. , A switch box 15, and a gyro sensor 19 (not shown in FIGS. 1 to 4; however, shown in FIG. 7). As shown schematically in the drawing, the mounting position of each device is such that the motor driver 11 is mounted below the middle storage section 1b of the bogie main body 1, the battery 13 is stored in the middle storage section 1b, and the upper storage section 1c. A control computer 12, a DC / DC converter 14, and a switch box 15 are mounted.
The gyro sensor 19 is a one-axis gyro sensor, and is disposed in a direction orthogonal to the axles 3a and 4a (that is, a direction in which the bogie body 1 tilts). Accordingly, the gyro sensor 19 detects the inclination angular velocity of the bogie main body 1.
The control computer 12 calculates a torque command value for the motors 5, 6 based on the output of the gyro sensor 19 and the encoder outputs of the motors 5, 6. The torque command value calculated by the control computer 12 is output to the motor driver 11, and the motor driver 11 controls the motors 5, 6 based on the output torque command value.
A body (not shown) of, for example, a robot is placed on the upper part 1d of the carriage body 1.
[0013]
The safety device 20 attached to the attachment portion 1e is for preventing the bogie main body 1 from falling when the inversion control is stopped, and employs a method in which the auxiliary wheels are moved up and down by a drive mechanism. Hereinafter, the safety device 20 will be described in detail with reference to FIGS. 5 and 6 are enlarged views for explaining the safety device 20. FIG. FIG. 5 shows a state in which the mobile trolley is operating (during normal running), and FIG. 6 shows a state in which the safety device 20 is operated when the mobile trolley is stopped (when the control device is stopped).
As shown in FIGS. 5 and 6, a supporting shaft 1f extending in parallel with the axles 3a, 4a is attached to the mounting portion 1e of the bogie main body 1, and auxiliary legs 21 extending in the front-rear direction of the bogie to the supporting shaft 1f. 22 is rotatably supported. The auxiliary legs 21, 22 include first limbs 21a, 22a on the base end side, and second limbs 21b, 22b bolted to the distal end side of the first limbs 21a, 22a with tapered members 21c, 22c interposed therebetween. Consists of Caster wheels 21d and 22d as auxiliary wheels are attached to the distal ends of the second limbs 21b and 22b. The tapered members 21c and 22c adjust the mounting angle between the first limb and the second limb, and the set height positions of the caster wheels 21d and 22d are adjusted by adjusting the tapered members 21c and 22c. It is possible.
Each of the second limbs 21b and 22b has a tip extending laterally in a substantially T-shape, and a caster wheel is attached to the tip of the second limb 21b so as to form a left-right pair (see FIGS. 1 and 3). ). That is, the movable cart is provided with caster wheels such that the caster wheels can be grounded at a total of four places in front, rear, left and right around a midpoint of a line connecting the ground points of the main wheels 3 and 4. The auxiliary legs 21 and 22 and the caster wheels 21d and 22d correspond to support members in claims.
[0014]
One end of each of the links 23a and 24a is rotatably connected to a pin near the base end of each of the first limbs 21a and 22a, and the other end of each of the links 23a and 24a is connected to the rod tip 25a of the actuator 25. ing. The actuator 25 is fixed to the chassis 1a. Therefore, when the actuator 25 operates to retract the rod, the auxiliary legs 21 and 22 rotate, and the caster wheels 21d and 22d are lifted as shown in FIG. On the other hand, when the actuator 25 operates to extend the rod downward, as shown in FIG. 6, the auxiliary legs 21 and 22 rotate to be in a state of being opened substantially straight, and the caster wheels 21d and 22d can come into contact with the road surface. To the right position.
[0015]
Next, a control system of the mobile trolley configured as described above will be described. FIG. 7 is a functional block diagram showing a configuration of a control system of the mobile trolley.
As shown in FIG. 7, the control of the mobile trolley is performed mainly by the control computer 12. The control computer 12 includes a CPU, a ROM, a RAM, and the like. The control computer 12 executes a control program stored in the ROM, thereby calculating a control command value related to the bogie translation direction. A first control command value calculating means), a bogie rotation direction control command value calculating means 28 for calculating a control command value relating to the bogie rotation direction (hereinafter simply referred to as a second control command value calculating means), and both control command values. A target value input means 27 for inputting a target value to the calculation means 26 and 28; a control command value addition means 29 for adding the control command values calculated by the two control command value calculation means 26 and 28; It functions as actuator switching means 70 for switching between a state in which the rod is expanded and a state in which the rod is contracted. Each means 26, 27, 28, 29, 70 constituted by the control computer 12 will be described later in detail. The processing performed by the control computer 12 can be emergency stopped by operating an emergency stop button 72 (not shown in FIGS. 1 to 4) and a remote control switch 74 provided on the bogie main body 1. ing.
A gyro sensor 19 is connected to the control computer 12, and the output of the gyro sensor 19 (the tilt angular velocity of the bogie main body 10) is input. The control computer 12 is connected to the motor drivers 11 and 11. The motor drivers 11, 11 are connected to the motors 5, 6, respectively, and drive the motors 5, 6 according to a torque command value from the control computer 12. The encoders 5c and 6c of the motors 5 and 6 are connected to the control computer 12, and outputs (rotation angles of the motors 5 and 6) from the encoders 5c and 6c are input to the control computer 12.
The actuator 25 is connected to the control computer 12 via a changeover switch 73. The changeover switch 73 is a switch that switches the state of the actuator 25 (the state where the rod is extended and the state where the rod is contracted). Specifically, when the operation signal from the control computer 12 is turned on, the changeover switch 73 is turned on, whereby the rod of the actuator 25 is contracted (that is, the caster wheels 21d and 22d are lifted). (See FIG. 5). Conversely, when the operation signal from the control computer 12 is turned off, the changeover switch 73 is turned off, whereby the rod of the actuator 25 is extended (that is, the caster wheels 21d and 22d are in contact with the road surface). (See FIG. 6). ON / OFF of the operation signal from the control computer 12 is performed by the actuator switching means 70.
The changeover switch 73 is also turned off by operating the emergency stop button 72 or the remote control switch 74. When these switches 72 and 74 are operated, the changeover switch 73 is turned off even if the operation signal output from the control computer 12 is on. Therefore, when the emergency stop button 72 or the remote control switch 74 is operated, the rod of the actuator 25 is immediately extended, and the caster wheels 21d and 22d come into contact with the road surface. When the power is turned off, the operation signal from the control computer 12 is turned off, so that the switch 74 is turned off. Therefore, the rod of the actuator 25 extends, and the caster wheels 21d and 22d come into contact with the road surface.
When the emergency stop button 72 or the remote control switch 74 is operated and the changeover switch 73 is turned off, the OFF state of the changeover switch 73 is input to the target value input means 27, and the target value input means 27 further increases the target value. The translation and rotation direction control command values to the cart are not output any more.
[0016]
Next, the first control command value calculating means 26 will be described. The first control command value calculating means 26 calculates a torque command value for controlling the movement of the bogie in the bogie translation direction. More specifically, a deviation between an output of the gyro sensor 19, a target value in the cart translation direction input by the target value input means 27 and a current value determined from the outputs of the encoders 5c and 6c is input, and the deviation is reduced. Calculates the torque command value of the motors 5 and 6 so that the motor maintains the inverted state.
In the present embodiment, the first control command value calculating means 26 is designed using H∞ control theory. By using the H∞ control theory, the first control command value calculating means 26 has robustness so that it can be stably inverted against disturbance. However, a control theory other than the H∞ control theory (for example, H₂The first control command value calculating means 26 can be designed using control theory, μ-design method, or the like. Even when designing using a control theory other than the H∞ control theory, it is preferable that the first control command value calculating means 26 has robustness so that it can be stably inverted against disturbance.
[0017]
An example of a design procedure of the first control command value calculating means 26 will be described. First, the movable trolley is modeled as a one-wheeled inverted pendulum viewed from the side (see FIG. 8). In FIG. 8, m1 is the mass of the vehicle body, J1 is the inertia around the center of gravity of the vehicle body, m2 is the mass of the wheel, J2 is the inertia around the axis of the wheel, and the distance from the axle to the center of gravity of the vehicle is 1. These parameters m1, j1, m2, j2, and l can be obtained by calculation or actual measurement. The inclination of the vehicle body from the vertical direction is η, and the rotation angle of the wheels is θ1.
Then, an equation of motion is created for one inverted pendulum shown in FIG. That is, when a motion equation is created assuming that the torque command value u is input to this control model, the motion equation is expressed by the following equation.
[0018]
(Equation 1)

[0019]
Next, the following equation is derived by linearizing assuming that η in the above-mentioned equation of motion is small and performing matrix display.
[0020]
(Equation 2)

[0021]
Therefore, the following equation of state is derived from the above equation.
[0022]
(Equation 3)

[0023]
Here, the observed amount y observed by the control system_gIs the first order derivative dη / dt of the inclination η of the vehicle body from the vertical direction, the rotation angle (θ1-η) of the wheel with respect to the vehicle body, and the first order derivative d (θ1-η) of this rotation angle (θ1-η). / Dt. That is, the observed quantity y_gIs represented by the following equation.
[0024]
(Equation 4)

[0025]
FIG. 10 shows the overall configuration of the control system modeled by the above-described procedure. As shown in FIG. 10, from the inverted pendulum model 41 to be controlled, dη / dt, (θ1−η), and d (θ1−η) / dt are observed as observation amounts. The observation noise 42 is added to the observed observation amount. Specifically, the observation noise n2 is added to the observation amount dη / dt, the observation noise n1 is added to the observation amount (θ1−η), and the observation noise n3 is added to the observation amount d (θ1−η) / dt.
The deviation between the observed value to which the observation noise 42 has been added and the target value 43 is input to the robust controller 44 (that is, the first control command value calculating means 26). Here, the target value “0” is set for the observation amount dη / dt, and the target value “(θ1-η) is set for the observation amount (θ1-η).^*Is the target value “d (θ1−η)” for the observed quantity d (θ1−η) / dt.^*/ Dt ”. Since the target value “0” is given to dη / dt (that is, the output of the gyro sensor 19) (that is, the inclination angular velocity of the vehicle body from the vertical direction is 0), the vehicle body maintains the inverted posture.
The robust controller 44 outputs a torque command value u. Then, the output torque command value u and disturbance w are input to the inverted pendulum model 41.
[0026]
Here, as the evaluation value for evaluating the control system, for example, the output u from the robust controller 36 or the rotation angle θ1 of the wheel can be used. The weighting function W46 for evaluating the output u and the evaluation function Q45 for evaluating the rotation angle θ1 can be finally determined by experiments, for example, by narrowing down to some extent by simulation or the like. In the present embodiment, the following functions are used.
[0027]
(Equation 5)

[0028]
In addition, for the specific design of the robust controller 44, various known control system design tools can be used.
[0029]
Next, the second control command value calculating means 28 will be described. The second control command value calculating means 28 calculates a torque command value for controlling the movement of the bogie in the bogie rotation direction.
Here, with respect to the bogie rotation direction, the moving bogie is modeled as a two-wheeled vehicle when viewed from directly above (see FIG. 9). In FIG. 9, φ represents the bogie rotation angle of the movable bogie, d represents the distance between the wheels 3, 4, and r represents the radius of the wheels. From the geometrical relationship shown in FIG. 9, the left and right wheel speeds (right wheel speed dθ)_L/ Dt, speed dθ of left wheel_R/ Dt) is converted into the Jacobian matrix for converting the speed (dx / dt, dy / dt) of the position of the moving carriage in the rectangular coordinate system and the angular velocity (dφ / dt) in the rotating direction of the carriage.
[0030]
(Equation 6)

[0031]
FIG. 11 shows the overall configuration of a control system by the second control command value calculating means 28. As is clear from FIG. 11, the second control command value calculating means 28 calculates the current position (x, y, φ) and the target position (x, y, φ).^*Is multiplied by a predetermined gain 50, the current speed [(dx / dt, dy / dt, dφ / dt)] and the target speed [(dx / dt, dy / dt, dφ / dt)^*) Is multiplied by a predetermined gain 52, and a torque command value T for controlling the motors 5 and 6 is calculated from the added value._R ^*, T_L ^*(So-called PD control is performed).
The movable carriage has two degrees of freedom as a position on a plane and one degree of freedom in the direction of rotation of the carriage, for a total of three degrees of freedom, but has only two actuators, motors 14 and 15 in total. For this reason, a value obtained by adding a value obtained by multiplying the above-mentioned position deviation by a gain 50 and a value obtained by multiplying the above-described position deviation by a gain 52 is used as a transposed Jacobi matrix J.^T54, the direct torque command value T_R ^*, T_L ^*Is calculated.
Also, the rotational angular velocity dθ of the right wheel 4_R/ Dt (ie, right wheel speed) and the rotational angular speed dθ of the left wheel 3_RThe current speed [(dx / dt, dy / dt, dφ / dt)] is calculated by multiplying / dt (that is, the left wheel speed) by the Jacobian matrix 56 (matrix shown in Expression 6).
Note that the position (x, y) and the speed (dx / dt, dy / dt) of the movable cart in the plane can also be controlled by the control system shown in FIG. However, since these are controlled by the first control command value calculating means 26, the gains (coefficients) for multiplying the position component (x, y) and the speed component (dx / dt, dy / dt) are used in the gains 50 and 52. ) Is “0”. Therefore, only the deviation of the bogie rotation angle φ of the movable bogie and the deviation of the bogie rotation angular velocity dφ / dt are used by the second control command value calculating means 28.
[0032]
As described above, the inverted pendulum control and the position control are simultaneously performed in the bogie translation direction (the direction perpendicular to the axle), and only the position control (bogie rotational angle φ) is performed in the bogie rotation direction (axle turning direction). These controls are performed so that they do not interfere with each other. The final control command value for the motors 5 and 6 is the sum of the control command value calculated by the first control command value calculation means 26 and the control command value calculated by the second control command value calculation means 28. It will be. That is, the control command value adding means 29 adds the control command value calculated by the first control command value calculating means 26 and the control command value calculated by the second control command value calculating means 28, and calculates the added value. Output to the motor drivers 11 and 11, respectively.
FIG. 12 shows the overall configuration of a control system including the above-described first control command value calculating means 26, second control command value calculating means 28, and control command value adding means 29. As is clear from FIG. 12, the control system of the present embodiment is such that the control of the translation direction of the bogie is incorporated in the control loop of the rotational direction of the bogie.
[0033]
Next, the target value input means 27 will be described. FIG. 13 is a block diagram showing the configuration of the target value input means 27. As shown in FIG. 13, the target value input unit 27 includes a target trajectory data storage unit 60, a translation direction target value calculation unit 62, and a rotation direction target value calculation unit 64.
[0034]
The target trajectory data storage means 60 stores the trajectory of the mobile trolley, the speed and acceleration of the mobile trolley at each position on the trajectory, and the target trajectory data that defines the angular velocity and angular acceleration of the mobile trolley in the rotation direction of the trolley. In the present embodiment, the trajectory of the mobile trolley is divided into sections in which uniform acceleration motion is performed (however, the case of zero acceleration (constant speed motion is also regarded as constant acceleration motion)), and the divided sections are (Section time t, section acceleration a, section initial velocity b, section angular acceleration a ', section angular initial velocity b') are set as target trajectory data. That is, assuming that the speed of the moving vehicle in the translation direction is v, the angular speed of the moving vehicle in the rotating direction of the vehicle is dφ / dt, and the elapsed time from the start of the section is t, these relationships are expressed by the following equations.
[0035]
(Equation 7)

[0036]
FIG. 14 shows an example of the trajectory of the mobile trolley and target trajectory data at that time. In the trajectory shown in FIG. 14, the acceleration a in the bogie translation direction is from point A to point B.₁Perform a uniform acceleration motion, and the speed and acceleration in the bogie rotation direction are “0”. From point B to point C, a constant speed motion is performed in the direction of translation of the truck, and the angular acceleration a in the direction of rotation of the truck.₁Perform an equiangular acceleration motion. From point C to point D, a constant velocity motion is performed in the direction of translation of the carriage, and a constant angular velocity movement is also performed in the direction of rotation of the carriage.
Therefore, the target trajectory data in the above-described case defines data defining the movement from point A to point B, data defining the movement from point B to point C, and defining the movement from point C to point D. It is composed of data. That is, the target trajectory data defining the movement from point A to point B is (t₁, A₁, 0,0,0), and the target trajectory data defining the movement from point B to point C is (t)₂, 0, a₁t₁, A₁′, 0), and the target trajectory data (t) that defines the movement from point C to point D₃, 0, a₁t₁, 0, a₁’T₂).
[0037]
The translation direction target value calculation means 62 calculates a target position (x, y) of the movable vehicle in the XY plane from the target trajectory data stored in the target trajectory data storage means 60.^*And target speed (dx / dt, dy / dt)^*Is calculated. For example, in the case where the exercise is started with the point A shown in FIG. 14 as the origin (0, 0), the time t from the start of the exercise (where 0 <t <t₁), The target position (x, y)^*= (A₁t²/ 2,0)^*And the target speed (dx / dt, dy / dt)^*= (A₁t, 0)^*Becomes
The rotation direction target value calculation means 64 calculates a target rotation angle (φ) of the movable vehicle from the target trajectory data stored in the target trajectory data storage means 60.^*And target speed (dφ / dt)^*Is calculated. Here, target rotation angle (φ)^*Means the total rotation angle of the mobile trolley. Therefore, even when the movable carts have the same posture (that is, the inclination from the traveling direction of the cart is the same), if the amount of rotation angle (the number of turns) is different, the target rotation angle (φ)^*Will also be different.
The target position (x, y, φ) calculated by the translation direction target value calculation means 62 and the rotation direction target value calculation means 64^*And target speed (dx / dt, dy / dt, dφ / dt)^*Is used as a target value of the first control command value calculation means 26 and the second control command value calculation means 28. That is, as shown in FIG. 12, the first control command value calculating means 26 (robust controller 41 shown in FIG. 12) provides the target position and the target speed with the bogie center position (θ1-η) and the bogie center speed d. (Θ1−η) / dt is used after being converted. The second control command value calculating means 28 has the above-mentioned target position (x, y, φ).^*(However, since the coefficient relating to x and y among the gains 50 is 0, actually φ^*Only used).
[0038]
Next, a process performed by the control computer 12 when the mobile trolley configured as described above is moved from the initial position to the final target position will be described with reference to FIG. FIG. 15 is a flowchart showing a processing procedure of the control computer 12. The trajectory for moving the mobile trolley from the initial position to the final target position is given by the target trajectory data described above.
When the control program is started, the control computer 12 first turns the operation signal output toward the changeover switch 73 from OFF to ON (S10). As a result, the changeover switch 73 is turned ON, and the rod of the actuator 25 operates so as to contract. For this reason, the caster wheels 21d and 22d, which have been in contact with the road surface, are lifted and rise to positions away from the road surface (see FIGS. 1 and 2). In this state, only the main wheels 3 and 4 are in contact with ground, and by controlling the rotation of the main wheels 3 and 4 by the control device, the bogie main body 1 can be kept in the inverted posture, and traveling with a small turn Can be. In addition, since the caster wheels 21d and 22d are lifted, the movable cart can easily run through even if there is a step on the road surface. It is preferable that the height positions of the caster wheels 21d, 22d are set to be suitable for the use environment (the unevenness of the road surface) by inserting and removing the taper member 22c in FIG. 5 in advance.
[0039]
Next, the control computer 12 determines whether the mobile trolley has reached the final target position (S12). Specifically, the determination is made based on whether or not the last data of the target trajectory data stored in the target trajectory data storage means 60 has been processed.
If the mobile trolley has reached the final target position (YES in step S12), the process proceeds to step S28, where the control computer 12 changes the operation signal output to the changeover switch 73 from ON to OFF (S28). , The process ends. As a result, the switch 73 is turned off, and the rod of the actuator 25 operates to extend. For this reason, the caster wheels 21d and 22d arranged at positions away from the road surface descend to the road surface (see FIGS. 3 and 4). In this state, the caster wheels 21d and 22d touch the road surface at positions before and after the ground point of the main wheels 3 and 4, and the bogie main body 1 stands still in an inverted posture without being greatly inclined.
In addition, even if the auxiliary legs 21 and 22 are directly in contact with the road surface, the fall can be prevented. However, in the present embodiment, the provision of the caster wheels facilitates subsequent movement, which is convenient for inspection work and the like.
[0040]
Conversely, if the movable trolley has not reached the final target position [NO in step S12], the process proceeds to step S14, where the values of the encoders 5c and 6c of the motors 5 and 6 (that is, the rotation angles of the wheels 3 and 4) are used. θ_R, Θ_L) Is read (S14).
Next, based on the values of the encoders 5c and 6c read in step S14, the wheel speed (dθ_R/ Dt, dθ_L/ Dt) and the current speed / current direction speed (dx / dt, dy / dt, dφ / dt) are calculated (S16). That is, the wheel speeds (dθ) of the wheels 3 and 4 are determined based on the temporal changes in the values of the encoders 5c and 6c._R/ Dt, dθ_L/ Dt), and the calculated wheel speed (dθ)_R/ Dt, dθ_L/ Dt) and the Jacobian matrix 56 to calculate the current speed / current direction speed (dx / dt, dy / dt, dφ / dt) (see FIG. 12).
In step S18, the output value dη / dt of the gyro sensor 19 is read.
In step S20, the target trajectory data stored in the target trajectory data storage means 60 is calculated based on the time t (ie, the elapsed time from the start of the trajectory control of the mobile trolley) after the control computer 12 is activated. Position (x, y, φ)^*And target speed (dx / dt, dy / dt, dφ / dt)^*Is calculated.
Since the current value and the target value of the movable trolley are calculated by the processing from step S14 to step S20, the torque command value T of the motors 5, 6 in the translation direction of the trolley is next calculated._R1, T_L1Are calculated (S22), and the torque command value T of the motors 5 and 6 in the bogie rotation direction is calculated._R2, T_L2Are calculated (S24). That is, the torque control value T_R1, T_L1Is calculated, and the torque command value T is calculated by the second control command value calculating means 28._R2, T_L2Is calculated.
In step S26, the torque command value T calculated in step S22_R1, T_L1And the torque command value T calculated in step S24_R2, T_L2And outputs these values to the corresponding motor drivers 11 and 11. As a result, the wheels 3 and 4 are driven. When step S26 ends, the process returns to step 12, and the process at the next control timing is started.
Note that the processing from step S12 to step S26 is performed at a predetermined time interval (for example, 0.5 ms), whereby the mobile trolley moves the trajectory defined by the target trajectory data at a predetermined speed / Exercise with acceleration, angular velocity, and angular acceleration.
[0041]
If the emergency stop button 72 or the remote control switch 74 is operated during the processing of steps S12 to S26 described above, the processing of the control computer 12 is stopped urgently, as described above, and , The changeover switch 73 is turned off. At this time, the processing of the control computer 12 is stopped and the inversion control is also stopped. However, when the changeover switch 73 is turned off, the caster wheels 21d and 22d are arranged at positions where they are in contact with the road surface. For this reason, the cart main body 1 can maintain the inverted posture without falling down.
[0042]
As is apparent from the above description, in the mobile trolley of the present embodiment, when the mobile trolley is normally running under the control of the control computer 12, the caster wheels 21d and 22d are lifted off the road surface and come into contact with the road surface. do not do. Therefore, the vehicle can run even if the road surface is inclined or there is a step.
On the other hand, when the process of the control computer 12 is completed and the inversion control is stopped, the caster wheels 21d and 22d are automatically arranged at the positions where they are in contact with the road surface, and the carriage main body 1 is maintained in an inverted state. Therefore, it is not necessary to perform the work of preventing the fall which is conventionally required by the operator.
Furthermore, even when the emergency stop button 72 or the remote control switch 74 is operated to stop the processing of the control computer 12 in an emergency, the caster wheels 21d and 22d are automatically arranged at positions where they touch the road surface, and the cart body 1 is turned over. To maintain. Therefore, the carriage main body 1 can be prevented from falling down, and the movable carriage can be prevented from being damaged.
[0043]
It should be noted that the present invention is not limited to the above-described embodiment at all, and can be implemented in various modified and improved forms based on the knowledge of those skilled in the art.
For example, the drive mechanism of the safety device for preventing the movable cart from falling can take various forms. Another example of the drive mechanism of the safety device will be described with reference to FIGS. In this example, a pusher is employed in place of the link mechanism described above. Since it has a symmetrical configuration, only one of them will be described including the illustration. FIGS. 16A and 16B are views for explaining the drive mechanism with the caster wheels lowered, wherein FIG. 16A is a partially cutaway front view, and FIG. 16B is a partially cutaway side view. FIG. 17 is a view for explaining the drive mechanism in a state where the caster wheel is lifted. (A) is a partially cutaway front view, and (B) is a partially cutaway side view.
As shown in the figure, auxiliary legs 31 and 32 are rotatably supported by a support shaft 1f provided on the mounting portion 1e of the bogie main body 1. A caster wheel 31d is attached to the tip of the auxiliary leg 31.
A pusher 35 and a pin pull-in device 36 are attached to the chassis 1a. 37 is a flange for attaching the pusher 35 to the chassis 1a. The pusher 35 has a built-in spring 35a, and the rod 35b is urged in the protruding direction by the built-in spring 35a. A roller 35c is attached to the tip of the rod 35b. When the rod 35b projects while the roller 35c is in contact with the middle of the auxiliary leg 31, the auxiliary leg 31 is opened in a substantially horizontal direction. For this reason, as shown in FIG. 16, the caster wheel 31d is switched to a predetermined position where it can be grounded to the road surface.
The pin pull-in device 36 is, for example, a linear actuator having a movable portion that can move forward and backward in a substantially horizontal direction, and an arm 36a extends from the movable portion. A support pin 36c as a locking member is formed at the tip of the arm 36a. On the other hand, the auxiliary leg 31 has a substantially horizontal locking hole 31a through which the support pin 36c can be inserted. At the position shown in FIG. 16, the support pin 36c is not engaged with the locking hole 31a.
When driving the movable trolley having the safety device having the above configuration, the auxiliary leg 31 is manually lifted in advance against the spring force of the built-in spring 35a, and the caster wheel 31d is separated from the road surface and retracted upward. In this state, as shown in FIG. 17, the support pin 36c at the tip of the arm 36a is inserted into the locking hole 31a, and the auxiliary leg 31 is held at a predetermined height position.
Thereafter, when the movable cart is stopped, the pin pull-in device 36 is operated from the state of FIG. 17 in which the caster wheel 31d is lifted, and the support pin 36c is moved in a direction to withdraw from the locking hole 31a. Thereby, the auxiliary leg 31 rotates to the position shown in FIG. 7 by the spring force of the pusher 35, and the caster wheel 31d comes into contact with the road surface. For this reason, the bogie main body 1 can maintain the inverted posture.
When such a mechanism is used, only the pin retracting device 36 is operated in the ON-OFF manner, so that a driving mechanism can be configured simply and inexpensively.
Note that the above pusher may be a single spring. Further, holding means may be provided that can hold the caster wheel at a position where the caster wheel contacts the road surface and at a position away from the road surface.
[0044]
Further, another example of the driving mechanism will be described with reference to FIGS. FIG. 18 is a side view of the mobile trolley showing a state where the caster wheels are lowered, and FIG. 19 is a plan view of the mobile trolley of FIG.
In this example, an air cylinder is employed instead of the link mechanism and the pusher described above. As shown in FIGS. 18 and 19, four air cylinders 37, 38, 39 and 40 are mounted on the bottom surface of the bogie main body 1. Since all four air cylinders 37, 38, 39, and 40 have the same configuration, only the configuration of the air cylinder 37 will be described here. As shown in FIG. 18, the air cylinder 37 has a rod 37a, and the rod 37a can move forward and backward with respect to the cylinder body. A caster wheel 37b is attached to the tip of the rod 37a. When the rod 37a is extended, the caster wheel 37b is arranged at a position where it comes into contact with the road surface, and when the rod 37a is contracted, the caster wheel 37b is arranged at a position away from the road surface.
As shown in FIG. 19, these four air cylinders 37, 38, 39, and 40 are respectively disposed two in front of and behind the main wheels 3, 4 with an interval left and right. That is, the air cylinders 37, 38, 39, and 40 are provided at a total of four positions in front, rear, left, and right with a center point of a line segment connecting the ground points of the main wheels 3 and 4 as a center.
When driving the movable trolley having the safety device having the above configuration, the rods are contracted by driving the air cylinders 37, 38, 39, and 40 in advance, and the caster wheels are separated from the road surface and retracted upward. Conversely, when stopping the movable cart, the air cylinders 37, 38, 39, and 40 are driven to extend the rods, and the caster wheels are brought into contact with the road surface. Thereby, the bogie main body 1 can maintain the inverted posture.
By employing a mechanism for directly moving the caster wheels up and down by an air cylinder or the like, the caster wheels can be quickly brought into contact with the road surface even in an emergency stop. Further, since only the rod of the air cylinder needs to be moved up and down, the installation space can be reduced. Further, since only the force in the compression direction acts on the rod supporting the vehicle weight of the bogie main body, the rod diameter can be reduced.
[0045]
The technical elements described in the present specification or the drawings exhibit technical utility singly or in various combinations, and are not limited to the combinations described in the claims at the time of filing. Further, the technology illustrated in the present specification or the drawings simultaneously achieves a plurality of objects, and has technical utility by achieving one of the objects.
[Brief description of the drawings]
FIG. 1 is a front view of a mobile trolley according to an embodiment.
FIG. 2 is a side view of the movable cart.
FIG. 3 is a front view for explaining a stopped state of the movable cart.
FIG. 4 is a side view for explaining a stop state of the movable cart.
FIG. 5 is an enlarged view for explaining the safety device.
FIG. 6 is an enlarged view for explaining the safety device.
FIG. 7 is a functional block diagram showing a configuration of a control system of the mobile trolley according to the embodiment.
FIG. 8 is a diagram in which a movable trolley is modeled in a translation direction.
FIG. 9 is a diagram illustrating a model of a movable trolley with respect to a rotation direction of the trolley.
FIG. 10 is a diagram showing a configuration of a control system relating to a translation direction of the movable cart according to the embodiment.
FIG. 11 is a diagram showing a configuration of a control system relating to a bogie rotation direction of the movable bogie according to the embodiment.
FIG. 12 is a diagram showing a configuration of a control system when the control system in the translation direction of the moving trolley and the control system in the rotation direction of the trolley of the present embodiment are combined.
FIG. 13 is a block diagram showing a configuration of a target value input unit.
FIG. 14 is a diagram showing an example of a target trajectory of the movable trolley and an example of target trajectory data for achieving the target trajectory;
FIG. 15 is an exemplary flowchart showing the procedure performed by the control computer of the embodiment.
FIG. 16 is a view for explaining another example of the drive mechanism of the safety device.
FIG. 17 is a view for explaining another example of the drive mechanism of the safety device.
FIG. 18 is a view for explaining still another example of the drive mechanism of the safety device.
FIG. 19 is a view for explaining still another example of the drive mechanism of the safety device.
[Explanation of symbols]
1: Bogie body
3, 4: Main wheels
5, 6: Motor
20: Safety device
21, 22: auxiliary legs (supporting members)
21d, 22d: caster wheel (supporting member)
25: Actuator
23a, 24a: Link

Claims (4)

A movable bogie comprising a wheel and a bogie main body supported by the wheel, wherein a center of gravity of the bogie main body is located above a rotation axis of the wheel,
Driving means for driving the wheels;
Control means for driving the driving means so that the bogie main body is maintained inverted;
A support member that is attached to the bogie main body and that can be switched between a first position that is separated from the ground point of the wheel in a substantially traveling direction and grounds on a road surface and a second position that is separated from the road surface,
A mobile trolley having a. The mobile trolley according to claim 1, further comprising a drive mechanism configured to switch the position of the support member between a first position and a second position. The drive mechanism arranges the support member at the second position when the control means is performing the inversion control of the bogie main body, and moves the support member to the second position when the control means is not performing the inversion control of the bogie main body. The mobile trolley according to claim 2, wherein the mobile trolley is arranged at a position 1. The mobile trolley according to any one of claims 1 to 3, wherein the support member has a caster wheel at a ground contact point.

Images