JPH0415713A

JPH0415713A - 平行２輪車の姿勢制御方法

Info

Publication number: JPH0415713A
Application number: JP2111802A
Authority: JP
Inventors: Kazuo Santo; 山藤　和男; Toshihisa Naruse; 成瀬　俊久; Yutaka Yoshinada; 裕吉灘
Original assignee: Komatsu Ltd
Current assignee: Komatsu Ltd
Priority date: 1990-05-01
Filing date: 1990-05-01
Publication date: 1992-01-21

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】〔産業上の利用分野〕本発明は、工場での自動化ラインにおけるサービスロボ
ット、家庭用ロボット等の移動機構として用いられる平
行２輪車の姿勢制御方法に関するものである。

〔従来の技術〕

平行２輪車は一種の倒立振子なので、制御を加えなけれ
ば起立を維持することができない。

従来のこの平行２輪車の姿勢制御は、例えば特公昭６３
−３０５０８２号公報に示されているような駆動輪のト
ルク制御を行なうようにしている。

〔発明が解決しようとする課題〕

上記従来の技術では、平行２輪車の起立維持と走行を独
立して制御することができなかった。

そのため、指定された位置への正確な移動は困難であっ
た。

本発明は上記のことにかんがみなされたもので、駆動輪
を起立維持制御に用いる必要がなくなり、平行２輪車の
走行速度、精度を大幅に改善でき、移動ロボットとして
の性能を飛躍的に向上させることが可能となり、また合
成重心フィードバックにより、多乱（こ対する安定性の
高い姿勢制御特性が得られ、平行２輪車に作業腕を設け
て作業を行なわせる等、外乱力が作用するような移動形
作業ロボットへの展開が可能となり、さらに、高加減速
が可能になって移動ロボットとしての機能を一段と高め
ることができ、そしてさらに、本体傾斜角補正方法によ
り、接触子を用いた簡便な本体傾斜角検出方法を用いて
も、連続的に傾斜の変化するような走行面への対応が可
能となるような平行２輪車の姿勢制御方法を提供するこ
とを目的とするものである。

〔課題を解決するための手段〕

上記目的を達成するために、本発明に係る平行２輪車の
姿勢制御方法は、下端部の両側に駆動輪を備えた上部構
造体と、この上部構造体の上部に振子状に設けた制御ア
ームと、上記駆動輪と制御アームをそれぞれモータにて
駆動して上記上部構造体を起立させて走行するようにし
た平行２輪車において、上部構造体と制御アームの合成
重心を常に平行２輪車と走行面との接地点上に保つよう
に制御アームの傾きを制御して上部構造体の静的起立安
定状態を保つようにする。

また上記制御条件下において、制御アームの角度を指定
して上部構造体の起立姿勢角を制御する。

また上記制御条件下において、上部構造体の角度を指定
して上部構造体の起立姿勢角を制御する。

また、車輪を含む上部構造体の平行移動時の慣性力を、
上部構造体と制御アームの合成重心を車輪の接地点から
ずらすことにより上記慣性力を補償する。

さらに、走行面との接触により上部構造体の傾斜角を検
出し、この検出傾斜角を合成重心位置情報により補正す
る。

〔作　用〕

制御アームを用いて起立維持制御を行なうことにより、
平行２輪車を起立維持制御と独立して駆動でき、指定位
置への停止や任意位置への高速移動が可動となる。

〔実　施　例〕

本発明の実施例を図面に基づいて説明する。

第１図は本発明方法に用いる平行２輪車の構成を示すも
ので、図中１は上部構造体、２はこの上部構造体１の上
部に振子状に設けた制御アーム、３，３は上部構造体１
の下端部の両側に設けた駆動輪である。４．５はそれぞ
れ制御アーム２及び駆動輪３の駆動用のモータである。

これらのモータ４，５はエンコーダを内蔵しており、出
力軸の回転角度が検出できるようになっている。それぞ
れのモータ４．５の出力はベベルギヤ機構６ａ、６ｂ、
スパーギヤ機構７ａ。

７ｂを介して制御アーム２及び駆動輪３を駆動するよう
になっている。８は上部構造体１の走行面に対する傾き
角を検出するエンコーダである。

次に上記構成の平行２輪車の姿勢制御方法を説明する。

（１）モデル化と運動方程式第１図における平行２輪車の車輪３を上部構造体１に固
定しく以下これを本体という）、第２図に示すように２
重倒立振子としてモデル化することにより得られた運動
方程式は次のようになる。

まず以下に説明する各式に用いる記号を示す。

ｍ、・本体質量［ＫｇｌＬ、・本体全長［ｓ　］ｍ２．アーム質ｊｌ　［ＫｇｌＬ２　アーム全長［ｉ　］ θ。：本体傾き角［ｒａｄ　］Ｕ、・車輪制御入力［Ｎｓｌ θ、・車輪回転角［ｒａｄ　］Ｕ２：アーム制御入力［ＮＩｌコＧ２　：アーム回転角［ｒａｄｌｇ、重力加速度Ｕ郡／ｓ　２コＪ＋’本体接地点回りの慣性モーメント［Ｋｇｍ　２コＪ２　：アーム回転軸回りの慣性モーメント［ＫｇＩ１
２コｅ１１本体接地点から重心までの距Ｍ［Ｉｌ］ｅ２・ア
ーム回転軸から重心までの距離［ｃ　］Ｃｏ二Ｃｏ二本
軸回転性係数［Ｎｍｓ　］Ｃ１二車輪軸の粘性係数［Ｎ
ｍｓコＣ２：アーム回転軸の粘性係数［Ｎｍｓ　］ｆｏ：本体
回転軸の摩擦トルク［Ｎｍ１ｆ、・車輪軸の摩擦トルク
［Ｎｍ］ｆ２　、アーム回転軸の摩擦トルク［ＮＩＩ］Ｒ車輪半
径［ＩＩ］Ｔｏ：移動距離［ｃｍ］本体の運動方程式％式％（］）アーム運動方程式Ｉ２２θ。＋Ｊ２′Ｇ２＋■１２θｏ２＋Ｇ２＋Ｃ２θ
２＋ｆ２″″Ｕ　２　　　　　　　　　　（２）ユニ（
こ、ＩＮ−Ｊ＋　＋Ｊ２　＋ｍ、ｒ−＋　２＋２ｍ２　　Ｌ
、　　Ｇ２　　ｃｏｓ　　Ｇ２１１２−ｍ２　Ｌ　１ｅ
　２　Ｓｉｎ　Ｇ２１２２−Ｊ２　＋ｍ２　ｔ、１ｅ７
ｅＯｓ　θ２Ｇｏ　−ｇ　　ｆｍｌｅ　１ｅＯ５θ０十
ｍ２（Ｌ　１ｃｏｓθ（、＋ｅ２ｃｏｓ（θθ＋θ２）
））Ｇ２−ｍ２　ｇ　Ｇ２　ＣＯＳ　　（θ０＋θ２）
Ｕｏ　−−（ｍ、＋ｍ２　）Ｒｇ　（θｏ　−π％　２
　）ここで、本体の制御人力Ｕ、は、図３に示すように
車輪半径Ｒの影響により、接地点か本体傾き角の関数と
してＧ、たけ変化することを補正するために見掛は上付
加した入力である。

（１１）起立安定化の制御方法姿勢安定化は制御アームのみて行ない、走行は車輪を制
御することにより行なう。車輪の回転による本体のバラ
ンスの崩れは外乱として扱い、それを制御アームを動作
させて吸収することにより姿勢を保つ。制御アームへの
制御人ノコは、合成重心を常に接地点上に保ち、静的バ
ランスをとることを目的とした合成重心フィードバック
（以下ＦＢと略記する）と、姿勢を制御するための姿勢
制御ＦＢにより構成され、その制御則を次式に示す。

Ｕ　　２　−　　Ｕ　　ｐｇ”　　Ｕ　　ｚ＋　　Ｕ　
　ｇＯ（３ン状聾と上方に上かっている場合の２つの状
態が（ｊ在する。この実施例では制御アームが常に水゛
１′、以下に下げられている状態のみを取り扱かう、二
の条件は次式で与えられる。

π　≦　００　＋　θ　２　≦　２　π　　　　　　　
　　　　　　　　　　　　　　　く４）（１）Ｘ（水下
面）射影合成重心ＦＢ制御法ｘ　゛＋ｉ面上に射影した
合成重心の位置を制御アムにフィードバックすることで
、接地点上に合成重心を保よう制御する方法である。

Ｃ：１ｊ　　御　Ｉリ　］　　　Ｕ、、−Ｋ、、Ｇｘ　
　　　　　　　　　　　　　　　　　　　　　（５）こ
こに、Ｕ、１９合成重心ＦＢ人力Ｕ、、−ｍ２　Ｇ２　ｇｃｏｓ　　（θＯ＋θ２）重力
補償入力以下に各ＦＢについてその種類と特徴を述べる。

［Ｉコ合成重心ＦＢ制御合成重心を接地点上に保つだめの制御アームの姿勢には
、制御アームか下方に下かっているＧｘ　　”　　［ｒ
１１＋　　ｅ　１ｃｏｓ　　Ｇ０　→−ｍ２ｆｅ２ｃｏ
ｓ（Ｇ０　＋θ、）＋Ｌ＋ｃｏｓ　　θｏ）ｌ］／（ｒ
ｎ　１＋ｍ２　　）　　　　　　　　　　　　　　　（
６）（Ｇｘ：Ｘ平面合成重心座標とする）（２）アーム角合成重心ＦＢ制御法合成重心を接地点上に保つためのθ。とＧ２の関係は幾
何学的関係により次式で表わされる。

この合成重心ＦＢ制御法は式（ア）で計算されるアーム
角θ０を目標値として、制御アームにＦＢを行なう方法
である。この方法ではθ２の計算にｃｏｓ−’を使用し
ているため、静的安定範囲外は計算不可能な状態となる
。

［制御則コＵ　ｐ、−Ｋ□（θ２゜−θ２）（８）ただ
し、０２ｇ３は式（７）で求められる合成重心ＦＢ制御
アーム目標角である。

［Ｉ［］姿勢制御のためのＦＢ制御起立状態の姿勢を指定するためのＦＢで、かならず合成
重心ＦＢとともに用いる。アーム、本体いずれかの角度
を指定するだけで、他方は合成重心ＦＢの働きにより釣
合の角度に導かれ平衡状態に収束する。

（ａ）アーム角ＦＢ制御法アーム角を指定することにより姿勢を決定する方法で、
本体角は理想的な場合、式（７）の関係を満たす。

［制御則］　Ｕ　ｘ−Ｋ　、−（θ２．−θ２）十に、
Σ（θ２．−０２）　　（９）ここに、Ｋ　ｐ　ａ　：アーム角比例ＦＢゲインに、：
アーム角積分ＦＢゲイン θ２、；アーム角目標値（ｂ）本体角ＦＢ制御法本体を指定角度に傾斜させることを目的とする方法。

［制御則］　Ｕ　ｘ−Ｋ−ｂ（θ。、−〇。）十に４．
Σ（θ０．−θｏ）　　（１０）ここに、Ｋ、ｂ：本体
角比例ＦＢゲインに１．：本体角積分ＦＢゲイン θ０、：本体角目標値（ｉｉｉ　）高速走行の制御方法走行時の車輪の回転による姿勢の乱れは、基本的には上
述（ｉｉ　）の［Ｉ］項の制御方法により、制御アーム
への外乱として吸収できるが、この方法では、移動加速
度に限界がある。

そこで、車輪に与える軌道から計算される慣性力を、フ
ィードフォーワード人力として制御アームにより補償す
ることで、さらに高加速度、高速度安定走行を実現する
方法を提案する。

慣性力の補償方法は、必要補償力相当分だけ合成重心位
置を接地点からずらすことにより実現する。

図４に示す力学的関係により、合成重心位置を接地点上
からずらすことにより、慣性力を補償する場合における
接地点回りの回転モーメントの釣合を考える。

［接地点回り回転モーメント釣合式］％式％）ａ：本体Ｘ座標加速度［■／Ｓ２コｇ１重力加速度［ｌ／Ｓ２］Ｇｘ　　合成重心Ｘ座標位置［匝］ＧＹ＝合成重心Ｙ座標位置［癲コこの式をＧｘについて解くと次式を得る。

ＹＧ、：　−・　ａ　　　　　　　　　　　　　　　　　
　　（１２）（１２）式で与えられるＧＸを合成重心フ
ィードバックの目標値とすることにより、慣性力を重力
により補償する動的合成重心フィードバック制御か実現
される。

ｃ制？Ｂｆｉｌｌコ制御アームへの制御人力Ｕ２、車輪への制御人力Ｕ、は
次式の制御則で与えられる。

Ｕ２＝Ｕ□＋Ｕ　ｘ＋　Ｕ　１ｔ（１３）Ｕ、−に、、
（θ８．−θ、）十に、＋（θ＋ｖｖ−θ−ｔ）　＋Ｋ
　、Σ（θ１．−０１）ユニに、Ｌ　Ｉｇ４成重心フィードバック人力Ｕ１．　　姿勢制御フィードバック人力Ｕ５、・重力補
償入力に、１：車輪比例ゲインに、、車輪速度ゲインに１．：車輪積分ゲイン θ８．車輪目標角Ｃｒａｄコ θ１．：車輪回転速度［ｒａｄ／ｓコ θ１１．−車輪回転速度目標値［ｒａｄ／ｓ　］各入力
成分は次式で与える。

Ｕ　ｐｇ””　Ｋ　ｐｇ　（Ｇ　ｘ、　　Ｇ　ｘ　）　
十Ｋｄ。

（ｆＧＸ−Ｇｘ）たたし、Ｋ、：合成重心比例フィードバック係数に、１：合成重
心微分フィードバンク係数ＣＸ　・合成重心Ｘ座標［ｍ
　］ｆＧｘ：１サンプリング時間前の合成重心Ｘ座標［ＩＩ
ｌ］Ａ：本体の加速度軌道指令Ｕ、・−Ｋｐ、（θＯ−θＯ）たたし、Ｋｐ　ｂ　’、本体角比例フィードバックゲインＵ　ｇ
ｔ−ｍ　２　　ｅ　２　　ｇ　ＣＯ５（θ０　＋θ２　
）（ｉｖ　）本体傾斜角補正方法本体傾斜角θ０を知る方法は、例えばロボット本体に取
付けた加速度センサて重力加速度の大きさを測定し、そ
の情報から傾き角を算出する等の非接触式の方法も考え
られるか、簡便で実用的な方法は図５に示すように、車
輪軸と同軸に取付けられたエンコーダ等の回転角センサ
９の回転軸１０に地面と接する接触子１１を設す、傾き
角を検出する方法である。

しかしこの方法で得られる情報は初期状態からの本体傾
斜角であるため、うねりを持った地面のように地面傾斜
角に変動がある場合には、本体傾斜角θ。に誤差が生ず
ることになり、そのままでは、うねりを持った曲面上を
走行することはできない。

そこで、以下に、上記のような場合の本体傾斜角補正方
法を提案する。

本体傾き角θ。にθ。の誤差が存在している場合の合成
重心位置ＧＸを、力の釣合関係より求める。Ｘ射影合成
重心フィードバックを用い姿勢安定化した場合について
考える。本体角θ０−π／２δθ。と表現し、δθ０を
本体の垂直からの傾斜角を表わす記号として使用する。

（図６謬照）Ｕ制御則］Ｕ　２　＝　Ｕ　、、＋　Ｕ　ｘ＋　Ｕ　、
。

＝　ｋ　ｐ−Ｘ　−十ｋ　ｐｈ　（θ。、−００）＋Ｕ
、。

一−Ｋｐ、Ｘ、＋に、ｈ（π／２− （π／２＋θ０．＋δθ。））＋ｍ２　ｅ２　ｇｅｏｓ　　（π／２＋θ。２＋δθ５
＋θ２）　　（１５）ここに、ＧＸ−ｆ　　（ｍ、ｅ　、＋ｍ２　　Ｌ　＋　　）ＣＯ
Ｓ　　（π／２＋　δ　θ０　＋　θ。、）＋ｍ２ｅ２
ｃｏｓ　　（π／２＋δ θ０＋θ。２＋θ２））／（ｍ＋＋ｍ２　）＝　−ｆ　　（ｍ、　　ｅｊ　　＋ｍ２　　Ｌ　、）ｓ
ｉｎ　　（δ　θ。　十　〇ｏ２）　＋ｍ２ｅ２ｓｉｎ
（δ　θ０　＋　θ。２十θ２）Ｉ　／　（ｍ、＋ｍ２
　）Ｕ釣合条（＃−Ｊ　Ｕ２−ｍ２　ｅ７　ｇｃｏｓ　　（
，７／２＋δθθ　＋θ２　　）　　　　　　（１７）
以上の関係より、式（１５）に式（１６）、（１７）を
代入し、δθ０と０２の満たす関係式を導くと次式をｉ
）る。

Ｋｐ、　ｌ　（ｍ＋　ｅ　１＋ｍ２Ｌ、　）　ｓｉｎ　
　（δθ。＋θｏ、）　　＋ｍ２　　ｅ２　ｓｉｎ　　
（δθ０　＋００計０２））／　（ｍ　、＋ｍ２　）　
　　ｋｏｂ　（θ。２＋δθ。）ｍ２　ｅ２　ｇｓｉｎ
　　（δθ０＋θｏ２＋０２）＋ｍ２ｅ２ｇｓｉｎ（δ
θ。十０２　）　　　　　　　　（１）また、合成重心
か接地点上に保たれる条件より、次式か与えられる。

Ｒ（π／２−（π／２＋δθｏ））１　（ｍｌ　ｅ　１＋ｍ２　Ｌ、＋　）　ｃｏｓ　　（
π／２＋δθｏ）　＋ｍ７　ｅ２　ｃｏｓ　（π／２キ
δθｏ十０２　）　ｌ　／　（ｍ　１＋ｒｎ２　）　　
　　　　　　（１９）この式をθ２について解くと次式
を得る。

θ２　−５ｊｎ　　−’　　［（（ｍ＋　　＋ｍ２　）
Ｒδθ。−（ｍ１ｅ　１　＋ｍ２　Ｌｌ　）　Ｓｉｎδ
θ。）７ｍ２　　ｅ２　　コ　　−　δ　θ　ｏ　　　
　　　　　　　　　　　　　　　　　　　　　　　（２
０）以上の計算により、本体角に誤差δθ。が存在する
場合の平衡状態における垂直からの本体傾斜角δθｏ１
制御アーム角θ２が、式（１８）。

（２０）よりもとめられ、この値を式（■６）に代入す
ることで合成重心位置Ｇが計算される。ここで注目され
ることは、本体傾斜角に誤差θ。２が存在すると、平衡
状態での合成重心位置ＧＸが原点に保たれなくなり、偏
差を生ずることである。

逆にとらえると、合成重心位置Ｇｘを、つねに原点に保
つよう本体角θ０を補正することで、実際の本体傾き角
を得ることが可能である。この原理を利用し、走行中、
合成重心位置を監視し、逐次本体角を補正する制御法に
より、従来不可能であったうねりをもつ曲面上の走行が
可能となる。また、この制御法を利用すると、本体角補
正量より、走行面の傾斜角がもとめられ、車輪回転角情
報とともに演算すると、走行面形状を再生することが可
能となる。また、この本体傾斜角補正量をメモリーに答
えることで、次回からはその情報を利用して高速に移動
することも可能となり、一種の学習制御を実現すること
か可能となる。

〔発明の効果〕

本発明によれば、合成重心フィードバックを用いること
により、駆動輪を起立維持制御に用いる必要がなぐなり
、平行２輪車の走行速度、精度を大幅に改善でき、移動
ロボットとしての性能を飛躍的に向上させることができ
る。また合成重心フィードバックにより、多乱に対する
安定性の高い姿勢制御特性が得られ、平行２輪車に作業
腕を設けて作業を行なわせる等、外乱力か作用するよう
な移動形作業ロボットへの展開か可能となる。

さらに、高速走行の制御方法を用いることにより、高加
減速が可能になって移動ロボットとしての機能を一段と
高めることができる。

そしてさらに、本体傾斜角補正方法により、接触子を用
いた簡便な本体傾斜角検出方法を用いても、連続的に傾
斜の変化するような走行面への対応が可能となる。

【図面の簡単な説明】

第１図は平行２輪車の構成を示す正面図、第２図は２重
倒立振子としてのモデル化を示す模式図、第３図は本体
角を接地点移動距離との関係を示す説明図、第４図は加
速時における力学関係を示す説明図、第５図は接触子を
用いた本体の角度検出状態を示す側面図、第６図は地面
が傾斜した場合の力学的釣合を示す説明図である。１は上部構造体、２は制御アーム、３は駆動輪、４，５
はモータ。

Claims

【特許請求の範囲】

（１）下端部の両側に駆動輪を備えた上部構造体と、こ
の上部構造体の上部に振子状に設けた制御アームと、上
記駆動輪と制御アームをそれぞれモータにて駆動して上
記上部構造体を起立させて走行するようにした平行２輪
車において、上部構造体と制御アームの合成重心を常に
平行２輪車と走行面との接地点上に保つように制御アー
ムの傾きを制御して上部構造体の静的起立安定状態を保
つようにすることを特徴とする平行２輪車の姿勢制御方
法。
（２）制御アームの角度を指定して上部構造体の起立姿
勢角を制御することを特徴とする請求項（１）記載の平
行２輪車の姿勢制御方法。
（３）上部構造体の角度を指定して上部構造体の起立姿
勢角を制御することを特徴とする請求項（１）記載の平
行２輪車の姿勢制御方法。
（４）車輪を含む上部構造体の平行移動時の慣性力を上
部構造体と制御アームの合成重心を車輪の接地点からず
らすことにより上記慣性力を補償するようにしたことを
特徴とする請求項（１）記載の平行２輪車の姿勢制御方
法。
（５）走行面との接触により上部構造体の傾斜角を検出
し、この検出傾斜角を合成重心位置情報により補正する
ようにしたことを特徴とする請求項（１）記載の平行２
輪車の姿勢制御方法。