(57)【特許請求の範囲】
【請求項１】
１つの車輪または同軸上に並んでいる複数の車輪を有し、かつ前記車輪に載置される部分の重心が車軸より上部に位置する倒立振子型移動装置であって、
前記車輪の回転状態を基に、外乱力の原因を判別し、前記判別結果に基づいて、前記車輪への出力トルクを決定する倒立制御部を
有し、
前記車輪の回転状態は、車輪角速度と、前記車輪に加えられる外乱力であり、
前記倒立制御部は、
以下の式（１）が成立している場合、前記外乱力の原因を空転と判別する
ことを特徴とする倒立振子型移動装置。
【数１】
0005873879.tif 000010
なお、τはモータトルク、Ｊは車輪の慣性モーメント、ｄφは車輪角速度、εは所定の値である。
【請求項２】
１つの車輪または同軸上に並んでいる複数の車輪を有し、かつ前記車輪に載置される部分の重心が車軸より上部に位置する倒立振子型移動装置であって、
前記車輪の回転状態を基に、外乱力の原因を判別し、前記判別結果に基づいて、前記車輪への出力トルクを決定する倒立制御部を
有し、
前記車輪の回転状態は、車輪角速度と、前記車輪に加えられる外乱力であり、
前記倒立制御部は、
前記車輪角速度および前記外乱力の方向が同じである状態から、所定時間内において、ともに逆方向となった場合、前記外乱力の原因をスリップと判別する
ことを特徴とする倒立振子型移動装置。
【請求項３】
１つの車輪または同軸上に並んでいる複数の車輪を有し、かつ前記車輪に載置される部分の重心が車軸より上部に位置する倒立振子型移動装置であって、
前記車輪の回転状態を基に、外乱力の原因を判別し、前記判別結果に基づいて、前記車輪への出力トルクを決定する倒立制御部を
有し、
前記車輪の回転状態は、車輪角速度と、前記車輪に加えられる外乱力であり、
前記倒立制御部は、
所定時間内において、前記車輪角速度の方向が逆方向に変化し、かつ、前記外乱力が存在する状態から、０へと変化した場合、
または、
所定時間内において、前記車輪角速度の方向が逆方向に変化し、かつ、前記外乱力が０から、前記外乱力が存在する状態に変化した場合、
前記外乱力の原因を段差と判別する
ことを特徴とする倒立振子型移動装置。
【請求項４】
１つの車輪または同軸上に並んでいる複数の車輪を有し、かつ前記車輪に載置される部分の重心が車軸より上部に位置する倒立振子型移動装置であって、
前記車輪の回転状態を基に、外乱力の原因を判別し、前記判別結果に基づいて、前記車輪への出力トルクを決定する倒立制御部を
有し、
前記車輪の回転状態は、車輪角速度と、前記車輪に加えられる外乱力であり、
前記倒立制御部は、
所定時間内において、前記車輪角速度の方向が前方へ移動する回転から、後方へ移動する回転に変化し、かつ、前記外乱力が前記車輪の回転方向に対して逆方向から、０へと変化した場合、
または、
所定時間内において、前記車輪角速度の方向が後方へ移動する回転から、前方へ移動する回転に変化し、かつ、前記外乱力が０から、前記車輪の回転方向に対して逆方向へと変化した場合、
前記外乱力の原因を前方上り段差と判別する
ことを特徴とする倒立振子型移動装置。
【請求項５】
１つの車輪または同軸上に並んでいる複数の車輪を有し、かつ前記車輪に載置される部分の重心が車軸より上部に位置する倒立振子型移動装置であって、
前記車輪の回転状態を基に、外乱力の原因を判別し、前記判別結果に基づいて、前記車輪への出力トルクを決定する倒立制御部を
有し、
前記車輪の回転状態は、車輪角速度と、前記車輪に加えられる外乱力であり、
前記倒立制御部は、
所定時間内において、前記車輪角速度の方向が前方へ移動する回転から、後方へ移動する回転に変化し、かつ、前記外乱力が前記車輪の回転方向に対して順方向から、０へと変化した場合、
または、
所定時間内において、前記車輪角速度の方向が後方へ移動する回転から、前方へ移動する回転に変化し、かつ、前記外乱力が０から、前記車輪の回転方向に対して順方向へと変化した場合、
前記外乱力の原因を前方下り段差と判別する
ことを特徴とする倒立振子型移動装置。
【請求項６】
１つの車輪または同軸上に並んでいる複数の車輪を有し、かつ前記車輪に載置される部分の重心が車軸より上部に位置する倒立振子型移動装置であって、
前記車輪の回転状態を基に、外乱力の原因を判別し、前記判別結果に基づいて、前記車輪への出力トルクを決定する倒立制御部を
有し、
前記車輪の回転状態は、車輪角速度と、前記車輪に加えられる外乱力であり、
前記倒立制御部は、
所定時間内において、前記車輪角速度の方向が後方へ移動する回転から、前方へ移動する回転に変化し、かつ、前記外乱力が前記車輪の回転方向に対して順方向から、０へと変化した場合、
または、
所定時間内において、前記車輪角速度の方向が前方へ移動する回転から、後方へ移動する回転に変化し、かつ、前記外乱力が０から、前記車輪の回転方向に対して順方向へと変化した場合、
前記外乱力の原因を後方上り段差と判別する
ことを特徴とする倒立振子型移動装置。
【請求項７】
１つの車輪または同軸上に並んでいる複数の車輪を有し、かつ前記車輪に載置される部分の重心が車軸より上部に位置する倒立振子型移動装置であって、
前記車輪の回転状態を基に、外乱力の原因を判別し、前記判別結果に基づいて、前記車輪への出力トルクを決定する倒立制御部を
有し、
前記車輪の回転状態は、車輪角速度と、前記車輪に加えられる外乱力であり、
前記倒立制御部は、
所定時間内において、前記車輪角速度の方向が後方へ移動する回転から、前方へ移動する回転に変化し、かつ、前記外乱力が前記車輪の回転方向に対して逆方向から、０へと変化した場合、
または、
所定時間内において、前記車輪角速度の方向が前方へ移動する回転から、後方へ移動する回転に変化し、かつ、前記外乱力が０から、前記車輪の回転方向に対して逆方向へと変化した場合、
前記外乱力の原因を後方下り段差と判別する
ことを特徴とする倒立振子型移動装置。
【請求項８】
１つの車輪または同軸上に並んでいる複数の車輪を有し、かつ前記車輪に載置される部分の重心が車軸より上部に位置する倒立振子型移動装置であって、
前記車輪の回転状態を基に、外乱力の原因を判別し、前記判別結果に基づいて、前記車輪への出力トルクを決定する倒立制御部を
有し、
前記車輪の回転状態は、車輪角速度と、前記車輪に加えられる外乱力であり、
前記倒立制御部は、
所定時間内に、前記外乱力の方向に変化はないが、前記車輪角速度の方向が逆方向に変化した場合、前記外乱力の原因を傾斜面と判別する
ことを特徴とする倒立振子型移動装置。
【請求項９】
１つの車輪または同軸上に並んでいる複数の車輪を有し、かつ前記車輪に載置される部分の重心が車軸より上部に位置する倒立振子型移動装置であって、
前記車輪の回転状態を基に、外乱力の原因を判別し、前記判別結果に基づいて、前記車輪への出力トルクを決定する倒立制御部を
有し、
前記車輪の回転状態は、車輪角速度と、前記車輪に加えられる外乱力であり、
前記倒立制御部は、
所定時間内において、前記外乱力の方向が、前記車輪の回転に対して逆方向のままで、前記車輪角速度の方向が逆方向に変化した場合、前記外乱力の原因を上り傾斜面と判別する
ことを特徴とする倒立振子型移動装置。
【請求項１０】
１つの車輪または同軸上に並んでいる複数の車輪を有し、かつ前記車輪に載置される部分の重心が車軸より上部に位置する倒立振子型移動装置であって、
前記車輪の回転状態を基に、外乱力の原因を判別し、前記判別結果に基づいて、前記車輪への出力トルクを決定する倒立制御部を
有し、
前記車輪の回転状態は、車輪角速度と、前記車輪に加えられる外乱力であり、
前記倒立制御部は、
所定時間内において、前記外乱力の方向が、前記車輪の回転に対して順方向のままで、前記車輪角速度の方向が逆方向に変化した場合、前記外乱力の原因を下り傾斜面と判別する
ことを特徴とする倒立振子型移動装置。
【請求項１１】
１つの車輪または同軸上に並んでいる複数の車輪を有し、かつ前記車輪に載置される部分の重心が車軸より上部に位置する倒立振子型移動装置であって、
前記車輪の回転状態を基に、外乱力の原因を判別し、前記判別結果に基づいて、前記車輪への出力トルクを決定する倒立制御部を
有し、
前記車輪の回転状態は、車輪角速度と、前記車輪に加えられる外乱力であり、
前記倒立制御部は、
前記車輪角速度および前記外乱力の方向が互いに逆方向の状態であり、所定時間内において前記車輪角速度および前記外乱力のそれぞれの方向が逆方向に変化した場合、前記外乱力の原因を柔らかい路面と判別する
ことを特徴とする倒立振子型移動装置。
【請求項１２】
前記倒立制御部は、
前記車輪角速度の方向が、所定時間内に変化している場合、車輪の回転方向の急激な変化が生じていると判別する
ことを特徴とする請求項１から請求項１１のいずれか一項に記載の倒立振子型移動装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、様々な路面上を走行する倒立振子型移動装置に関する。
【背景技術】
【０００２】
車輪により地面に接し、１つの車輪または同軸上に並んでいる複数の車輪を有し、かつ車輪以外の部分の重心（車輪に載置される部分の重心）が車軸より上部に位置する倒立振子型移動装置は、静的には安定しておらず、移動を伴う動的な制御により安定性を保つため、路面の影響を大きく受ける。このような場合、倒立振子型移動装置が路面影響により挙動が不安定になるのを防ぐための背景技術として、例えば、特許文献１や、特許文献２に記載の技術が開示されている。
【０００３】  特許文献１には、左右一対の車輪と、それぞれの車輪を駆動するモータにおいて、それぞれのモータの出力トルクを、モータ毎に負荷されている外乱トルクに応じて決定する自走車とその制御装置および制御方法が開示されている。この技術による自走車は、外乱トルクが負荷された場合でも、正しく走行し続けることができる。【０００４】  また、特許文献２には、車体の姿勢変化を考慮して路面勾配を推定し、その路面勾配に応じて駆動トルクを補正する車両が開示されている。この技術による車両は、路面の勾配に関わらず、安定した停止状態や、走行状態を実現することができる。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００８−２６３６７６号公報
【特許文献２】特開２００９−４０３６５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】  倒立振子型移動装置は、その底面積が小さいことから、人間が搭乗する移動体として利用するケースや、サービスロボットとして屋内の人の生活空間を移動するケースなどがある。このように、倒立振子型移動装置は人間と共存しながらあらゆる環境で活躍することが期待されている。このような状況下で倒立振子型移動装置を活用する場合には、転倒の防止や乗り心地や安定性が重要となり、上体の揺れと振れ幅を小さく抑えることが必要である。【０００７】  倒立振子型移動装置はバランスを保つために小刻みに前後方向に加速する。しかし、安定化動作に必要な車輪の挙動は路面の影響を大きく受け、特に車輪回転速度の方向が頻繁に切り替わるような低速度の時に揺れの抑制が難しい場合がある。例えば、草地や絨毯などの柔らかい路面では、車輪の動きを妨げるような外乱力が車輪に発生し、この外乱力は車輪回転速度の方向に応じて方向を変える。つまり、柔らかい路面では、車輪が沈み込んでしまうため、前後左右のどれに移動しようとしても、常に車輪に対して抵抗力が加えられる。このため、倒立振子型移動装置における小刻みに前後方向に加速する動作が阻害され、その動作が不安定になると言う問題がある。【０００８】  しかしながら、外乱力を補正する従来技術では柔らかい路面や、斜面などの様々な種類の外乱力に対して十分に補償する技術が開示されていない。つまり、特許文献１および特許文献２などの従来技術では、一種類の外乱力に対応できるようになっているものの、対象外の外乱力が加えられると対応不可能となってしまう。【０００９】  例えば、路面から加えられた外乱力を外乱オブサーバで推定して路面傾斜と段差に対応する特許文献１記載の技術と、路面傾斜を推定する技術が記載されている特許文献２記載の技術を用いた場合について説明する。想定している路面傾斜と段差による外乱力は移動体の車輪速度に関わらず一方向である。一方、倒立振子型移動装置が柔らかい路面上で停止または低速度で移動している場合においては車輪速度の方向が大きく変わり外乱力の方向が激しく変化する。このため、一方向の外乱力を仮定する外乱オブザーバを用いて、柔らかい路面に対応しようとすると、車輪速度の切り替わり時に外乱力を強める方向に補正を行ってしまう場合があり、結果として、倒立振子型移動装置の揺れが増幅してしまうことがある。【００１０】  つまり、特許文献１に記載の技術を用いて、特許文献２に記載の技術を実現することは困難である。特許文献２に記載の技術を用いて、特許文献１に記載の技術を実現することも同様である。つまり、従来技術を単純に組み合わせて、様々な種類の外乱力に対応することは困難である。【００１１】
このような背景に鑑みて本発明がなされたのであり、本発明は、様々な外乱力に対応できる倒立振子型移動装置を提供することを課題とする。
【課題を解決するための手段】
【００１２】
前記課題を解決するため、本発明は、１つの車輪または同軸上に並んでいる複数の車輪を有し、かつ前記車輪に載置される部分の重心が車軸より上部に位置する倒立振子型移動装置であって、前記車輪の回転状態を基に、外乱力の原因を判別し、前記判別結果に基づいて、前記車輪への出力トルクを決定する倒立制御部を有することを特徴とする。そして、前記車輪の回転状態は、車輪角速度と、前記車輪に加えられる外乱力であり、τをモータトルク、Ｊを車輪の慣性モーメント、ｄφは車輪角速度、εを所定の値としたとき、||（τ／Ｊ）−（ｄ／ｄｔ）ｄφ||＜εであるとき、前記倒立制御部は、前記該欄力の原因を空転と判別することを特徴とする。
その他の解決手段については、実施形態において記載する。
【発明の効果】
【００１３】
本発明によれば、様々な外乱力に対応できる倒立振子型移動装置を提供することができる。
【図面の簡単な説明】
【００１４】
【図１】本実施形態に係る倒立振子型移動装置の構成概略図である。
【図２】本実施形態に係る倒立振子型移動装置の機能ブロック図である。
【図３】本実施形態に係る倒立制御部による倒立制御処理の手順を示すフローチャートである。
【図４】本実施形態に係る外乱原因の判別および指示トルクの算出の処理手順を示すフローチャートである。
【図５】スリップに対する判別条件を示す表である。
【図６】段差に対する判別条件を示す表である。
【図７】斜面に対する判別条件を示す表である。
【図８】柔らかい路面に対する判別条件を示す表である。
【発明を実施するための形態】
【００１５】  次に、本発明を実施するための形態（「実施形態」という）について、適宜図面を参照しながら詳細に説明する。なお、各図面において、同様の構成要素については、同一の符号を付して説明を省略する。
【００１６】
［装置構成］
図１は、本実施形態に係る倒立振子型移動装置の構成概略図である。
図１（ａ）は本実施形態に係る倒立振子型移動装置の側面図であり、図１（ｂ）は倒立振子型移動装置の正面図である。
図１（ａ）および図１（ｂ）に示すように、倒立振子型移動装置１は左右１対からなる同軸上の車輪４Ｒ，４Ｌ（４）と、移動機構部３と、それ以外の上体部とで構成されている。倒立振子型移動装置１において、車輪４Ｒ，４Ｌは同軸上に並んでおり、かつ、前記車輪４以外の部分の重心（車輪に載置される部分の重心）が車軸より上部に位置している。本実施形態では、車輪４は２つとしているが、１つ以上であればよい。
移動機構部３は機構制御部３２と、車輪４Ｒ，４Ｌのそれぞれを回転させるモータ３１Ｒ，３１Ｌ（３１）を備えている。車輪４Ｒ，４Ｌは、それぞれ独立のモータ３１Ｒ，３１Ｌに接続されている。モータ３１Ｒ，３１Ｌは機構制御部３２に接続され、それぞれ所定のトルクを発生させるように制御される。車輪４Ｒ，４Ｌの車輪角度φＲ，φＬと、車輪角速度ｄφＲ，ｄφＬはモータ３１Ｒ，３１Ｌを介して機構制御部３２が取得する。以後、単に、車輪角度φを車輪角度φＲ，φＬの左右平均、車輪角速度ｄφを車輪角速度ｄφＲ，ｄφＬの平均として適宜扱うことがある。ここで、車輪角速度は、車輪軸回りの角速度である。
【００１７】  上体部は移動機構部３の上部に備えられている。上体部は、後記する各演算を行うための計算部２と、車輪４Ｒ，４Ｌを除く倒立振子型移動装置１の重心位置と車軸を結んだ直線と鉛直方向のなす傾斜角度θ、および傾斜角速度ｄθを測定するための姿勢センサ５（例えば、ジャイロセンサ）を備えている。なお、計算部２と移動機構部３と姿勢センサ５とは情報交換ができるように接続されている。
【００１８】
図２は、本実施形態に係る倒立振子型移動装置の機能ブロック図である。
図２に示すように、計算部２は、行動決定部２１、経路計画部２２、速度計画部２３、倒立制御部２４を備えている。また、計算部２は、姿勢センサ５や、移動機構部３に接続されている。なお、倒立制御部２４の処理が本実施形態に特徴的なものである。
ここで、計算部２および各部２１〜２４は、図示しないＲＯＭ（Read Only Memory）に格納されているプログラムが、ＣＰＵ（Central Processing Unit）によって実行されることによって具現化する。
【００１９】  以下、経路計画生成の手法を簡単に説明する。なお、経路計画は倒立振子型移動装置１が自律型である場合には必要であるが、人が搭乗して操縦する搭乗型である場合は省略可能である。  行動決定部２１は倒立振子型移動装置１の当面の移動目標を決定する。当面の移動目標の情報（移動目標情報）は移動先位置ｐｒ、移動先方向ωｒ、最大移動速度Ｖｍａｘ、最大車輪トルクτｍａｘの各情報を含んでいる。これらの情報は、予めプログラムされており、移動先位置に達する毎に次の移動目標がプログラムにより設定されるようになっている。また、行動決定部２１は、倒立制御部２４から、現在位置を取得する。また、行動決定部２１は、車輪４Ｒ，４Ｌに加えられる推定外乱力の原因が段差であることを示す情報が含まれるとともに、段差が存在する方向（前方上り段差など）に関する情報（段差種の情報）が含まれている段差シグナルを倒立制御部２４から取得した場合、現在位置ｐと移動先位置ｐｒの距離が予め設定された距離より近い場合は、不安定さを回避するため、移動目標の移動先位置ｐｒを段差シグナルが示す段差の逆方向に予め設定された距離の分動かす。
【００２０】  経路計画部２２は、行動決定部２１から移動目標を、倒立制御部２４から現在位置ｐと現在の旋回方向ωを取得し、当面の経路を生成する。当面の経路生成方法は、一般的には、現在位置ｐ、移動先位置ｐｒを直線で結ぶ方法が用いられる。または、当面の経路生成方法として移動先位置ｐｒと現在位置ｐにおいて目標とする旋回方向ωｒと現在の旋回方向ωが異なる場合は現在位置ｐと移動先位置ｐｒを滑らかに接続するベジェ曲線やスプライン曲線を当面の経路とする手法が用いられてもよい。また、経路計画部２２は生成した当面の経路情報を経路上にある点の位置座標データ（ｘ，ｙ，ω）群として、速度計画部２３に送信する。
【００２１】  速度計画部２３は行動決定部２１から当面の移動目標を取得し、経路計画部２２から経路計画を取得し、姿勢センサ５から上体の傾斜角度θと傾斜角速度ｄθを取得し、移動機構部３から倒立制御部２４を介して車輪角速度ｄφを取得し、取得した各情報を基に経路全体の速度計画を生成する。また、速度計画部２３は生成した経路全体の速度計画と、取得した全体の経路計画を倒立制御部２４へ送信する。
【００２２】  速度計画は並進に関するものと旋回に関するものからなり、並進に関する速度計画は、例えば特開２００７−３１９９９１号公報に記載されている方法で生成することができる。この方法において、倒立振子型移動装置１は、自身の運動方程式を利用して、車輪角度φと傾斜角度θの線形和である特徴点ｑを求める。そして、倒立振子型移動装置１は、これを複数回微分したものを媒介変数として移動目標を表現することで決定する。さらに、倒立振子型移動装置１は、車輪角度φ、車輪角速度ｄφ、傾斜角度θ、傾斜角速度ｄθからなる速度計画を生成する。この時、目標とする速度計画全体の移動距離には全体の経路計画に沿った各当面の移動目標位置までの道のりが用いられる。次に、倒立振子型移動装置１は、生成された並進に関する速度計画と経路計画から旋回に関する速度計画を算出する。旋回に関する速度計画は経路上で取るべき旋回方向ωと旋回方向速度ｄωからなる。経路計画を構成する経路上にある点の位置座標データをｐｒ［ｉ］（ｉはデータ番号）とし、時刻ｔでの倒立振子型移動装置１が取るべき場所ｐｒ［ｋ］とするとｋは、以下の式（１）を最小にするものが探索されることによって決定される。
【００２３】
【数１】
0005873879.tif 000002
【００２４】 0005873879.tif 000003
り同時刻の速度計画での車輪角速度ｄφｒ(j)に車輪４の半径ｓを乗じたものである。また、ｔ０は計画開始時点での時刻である。また時刻ｔにおける（＝位置座標データｐｒ［ｋ］に対応する）旋回方向速度ｄωｒ［ｋ］は、式（２）により決定する。
【００２５】
【数２】
0005873879.tif 000004
【００２６】  ここで、Ｒ［ｋ］は旋回半径である。Ｒ［ｋ］は、ｐｒ［ｋ−１］、ｐｒ［ｋ］、ｐｒ［ｋ＋１］の３点のｘ、ｙ座標を以下の式（３）に代入し、連立させることで旋回円の中心座標ａ、ｂと供に算出される。
【００２７】  （ｘ−ａ）２＋（ｙ−ｂ）２＝Ｒ［ｋ］２  ・・・  （３）
【００２８】  以上により旋回方向と旋回方向速度の時刻ｔにおける目標値からなる旋回に関する速度計画が算出される。
【００２９】  倒立制御部２４は速度計画と経路計画、現在の状態、推定外乱力からモータ３１への指示トルクτｒを算出し、機構制御部３２へ送信する。倒立制御部２４の処理は、図３以降で詳細に説明する。
【００３０】［倒立制御処理］  図３は、本実施形態に係る倒立制御部による倒立制御処理の手順を示すフローチャートである。  まず、倒立制御部２４は速度計画部２３から速度計画や、経路計画などを取得し、さらに移動機構部３から車輪４Ｒ，４Ｌの回転角度φＲ，φＬや、車輪４Ｒ，４Ｌの回転角速度ｄφＲ，ｄφＬや、直前に出力された車輪４Ｒ，４ＬのモータトルクτＲ，τＬなどといった各種情報を取得する（Ｓ１０１）。  次に、倒立制御部２４は倒立振子型移動装置１の現在位置と旋回方向を更新する（Ｓ１０２）。更新は、以下の式（４）に基づいて行われる。
【００３１】
【数３】
0005873879.tif 000005
【００３２】  ここでのｔは時刻であり、ｔ０は現在時刻である。また、車輪角度φＲ，φＬと回転角速度ｄφＲ，ｄφＬは過去データを用いて時刻ｔの関数で記述される。そして、ｖは倒立振子型移動装置１の移動速度であり、ｓは車輪４の半径であり、ｆは車輪４Ｒ，４Ｌの間隔である
【００３３】  次に、倒立制御部２４は、並進用モータトルクτｓを以下の式（５）を用いて算出する（Ｓ１０３）。
【００３４】
【数４】
0005873879.tif 000006
【００３５】  ここで、Ｋｊ（ｊ＝１、２、３、４）はスカラ量であり、ヒューリスティックに設定されたり、またＬＱＲ（Linear Quadratic Regulator）やＨ∞制御論により設定されたりする値である。また、θｒ，ｄθｒ，φｒ，ｄφｒはそれぞれ経路計画から得られた時刻ｔにおける傾斜角度目標値、傾斜角速度目標値、車輪角度目標値、車輪角速度目標値である。
【００３６】  続いて、倒立制御部２４は経路に追従するための旋回用モータトルクτｃを以下の式（６）を用いて算出する（Ｓ１０４）。
【００３７】
【数５】
0005873879.tif 000007
【００３８】  ここで、Ｋｊ（ｊ＝５、６、７）はスカラ量であり、ヒューリスティックに設定されたり、または前記したような各種制御理論により設定されたりする値である。また、Ｈは倒立振子型移動装置１の現在位置ｐ（ｔ０）に最も近い経路計画上の点までの距離である。
【００３９】  そして、倒立制御部２４は左右の車輪４Ｒ，４Ｌそれぞれに加えられる外乱力ＤＲ，ＤＬを推定する（Ｓ１０５）。推定される外乱力（以降、推定外乱力と適宜称する）とは、外部から車輪４に加えられる量である。  推定外乱力を推定するためには、左右それぞれの車軸にトルクセンサを取り付け、モータ３１Ｒ，３１Ｌの出力と、過去に推定されたモータ３１Ｒ，３１Ｌの出力との比較で算出することもできるが、外乱推定オブザーバ（不図示）を用いて推定してもよい。なお、外乱力の推定は、公知の技術であるため詳細な説明を省略する。
【００４０】  倒立振子型移動装置１の運動方程式から算出される状態量には、車輪角度φや、車輪角速度ｄφや、傾斜角度や、傾斜角速度などがあるが、外乱推定オブザーバは倒立振子型移動装置１に対し推定外乱力を状態に加えた拡大システムのオブザーバを構成することにより推定外乱力を得る。外乱推定オブザーバの設計法は一般によく知られたものを用いる。また外乱推定オブザーバは車輪４Ｒ，４Ｌそれぞれについて設定される。左右それぞれの外乱推定オブサーバは、倒立振子移動体の状態量である、車輪４Ｒ，４Ｌそれぞれの車輪角度φＲ，φＬや、車輪角速度ｄφＲ，ｄφＬや、傾斜角度や、傾斜角速度と車輪角度と対応するモータ３１に直前に指定したそれぞれのトルクを入力として与えることで推定外乱力ＤＲ，ＤＬの大きさと車輪４Ｒ，４Ｌのどの方向に加えられているかを推定する。
【００４１】  次に、倒立制御部２４は推定外乱力ＤＲ，ＤＬの大きさや、方向や、車輪角速度ｄφＲ，ｄφＬや、直前に出力したモータトルクτＲ，τＬなどを用いて推定外乱力の原因（外乱原因）の判別を行い、判別された外乱原因に応じた左右モータ３１それぞれへの指示トルクτｒＲ，τｒＬを算出する（Ｓ１０６）。ステップＳ１０６の詳細は、図４で後記する。  そして、倒立制御部２４は、ステップＳ１０６で算出された指示トルクτｒＲ，τｒＬを移動機構部３へ出力し（Ｓ１０７）、処理を終了する。
【００４２】［外乱原因の判別および指示トルクの算出：図３のステップＳ１０６］  図４は、本実施形態に係る外乱原因の判別および指示トルクの算出（図３のステップＳ１０６）の処理手順を示すフローチャートである。なお、図４におけるフローチャートは、左右の車輪４Ｒ，４Ｌのうち、一方の車輪４（ここでは、右車輪４Ｒとする）に対する処理であり、他方の車輪４における同様の処理も並列かつ独立に行われている。  まず、倒立制御部２４は、車輪４Ｒが完全に空転しているか否かを判定する（Ｓ２０１）。車輪４Ｒが完全に空転している場合（つまり、車輪４Ｒが接地していない場合）、倒立振子型移動装置１の上体部における負荷分を考慮しなくてよくなるため、車輪４Ｒに加えたトルクを増加させる方に推定外乱力ＤＲが加えられる。このことより、倒立制御部２４は、以下の式（７）が成り立っている場合、空転が生じていると判定する。
【００４３】
【数６】
0005873879.tif 000008
【００４４】  ここで、τＲは直前に加えられた実際のトルク（実測値）であり、Ｊは車輪４Ｒの慣性モーメント、εは許容誤差範囲である。式（７）が真である場合、すなわち、倒立制御部２４が車輪４Ｒに空転が生じていると判定した場合（Ｓ２０１→Ｙｅｓ）、空転から復帰した後に速やかに制御行うため、モータ３１への指示トルクτｒＲは推定外乱力がない場合よりも減少させることが望ましく、倒立制御部２４は、例えば、以下の式（８）を用いて、空転時におけるモータ３１Ｒへの指示トルクτｔＲを算出し（Ｓ２０２）、図３のステップＳ１０７へリターンする。
【００４５】τｒＲ＝τｓ＋τｃ−Ｋ２（φ−φｒ）−Ｋ４（ｄφ−ｄφｒ）・・・（８）
【００４６】  ここで、Ｋ２、Ｋ４は並進用モータトルクτｓの算出時（図３のステップＳ１０３）で用いた値であり、式（８）の第３項以降の効果により、車輪４Ｒの空転による目標車輪角度φｒ、現在の車輪角度φｒ、目標車輪角速度ｄφｒおよび現在の車輪角速度ｄφｒの偏差の過剰拡大による式（８）の第２項までの過剰なトルク要求を緩和できる。
【００４７】  ステップＳ２０１の結果、空転が生じていないと判定された場合（Ｓ２０１→Ｎｏ）、すなわち、式（７）が満たされていない場合、倒立制御部２４は車輪角速度ｄφＲが予め設定された過去の一定時間内において符号＋から０になったり、符号−から０になったり、０を超えて符号を反転させるような変化を行っているか否かを判定する。つまり、倒立制御部２４は、車輪角速度ｄφＲの方向が急激に変化しているか否かを判定する（Ｓ２０３）。  ステップＳ２０３の結果、車輪角速度ｄφＲの方向が急激に変化していない場合（Ｓ２０３→Ｎｏ）、倒立制御部２４は、以下の式（９）を用いて、急激な変化を伴わない時におけるモータ３１Ｒへの指示トルクτｒＲを算出し（Ｓ２０４）、図３のステップＳ１０７へリターンする。
【００４８】τｒＲ＝τｓ＋τｃ−ＤＲ・・・（９）
【００４９】  式（９）により、倒立制御部２４は車輪角速度ｄφＲが０を跨いで変化しないことにより、車輪角速度ｄφＲの符号に由来する早い外乱力の符号変化がないとして、モータ３１Ｒへの指示トルクτｒＲへ外乱力を相殺するように補正している。
【００５０】  ステップＳ２０３の結果、車輪角速度ｄφＲの方向が急激に変化している場合（Ｓ２０３→Ｙｅｓ）、倒立制御部２４はスリップが生じているか否かを判定する（Ｓ２０５）。倒立制御部２４は、スリップが生じているか否かを後記する図５に示す表に基づいて判定する。
【００５１】  ステップＳ２０５の結果、スリップが生じていると判定された場合（Ｓ２０５→Ｙｅｓ）、倒立制御部２４は、以下の式（１０）に基づいて、スリップ時におけるモータ３１Ｒへの指示トルクτｒＲを算出し（Ｓ２０６）、図３のステップＳ１０７へリターンする。
【００５２】τｒＲ＝Ｇ（τｓ＋τｃ）・・・（１０）
【００５３】  ここで、Ｇは１以下０以上の値をとる、予め設定されたスカラ量であり、モータトルクを減少させることでスリップの早期解消の働きをする。
【００５４】  ステップＳ２０５の結果、スリップが生じていないと判定された場合（Ｓ２０５→Ｎｏ）、倒立制御部２４は、推定外乱力ＤＲの原因が段差であるかを判定する（Ｓ２０７）。  推定外乱力ＤＲの原因が段差であるか否か判定は、車輪角速度ｄφＲと推定外乱力ＤＲが後記する図６の条件を満たしているかによって行われる。
【００５５】  推定外乱力ＤＲの原因が段差であると判定された場合（Ｓ２０７→Ｙｅｓ）、倒立制御部２４は、図６の表に従って、前方上り段差であるか、前方下り段差であるか、後方上り段差であるか、後方下り段差であるかといった段差種の判定を行う（Ｓ２０８）。そして、倒立制御部２４は、判定した段差種の情報が格納されている段差シグナルを生成し、生成した段差シグナルを行動決定部２１へ送信する。  また、倒立制御部２４は、段差時のモータ３１Ｒへの指示トルクτｒＲを式（９）に基づいて算出し（Ｓ２０９）、図３のステップＳ１０７へリターンする。
【００５６】  ステップＳ２０７の結果、推定外乱力ＤＲの原因が段差ではないと判定された場合（Ｓ２０７→Ｎｏ）、倒立制御部２４は推定外乱力ＤＲの原因が路面傾斜によるものか否かを判定する（Ｓ２１０）。推定外乱力ＤＲの原因が路面傾斜によるものか否かの判定は車輪角速度ｄφＲと推定外乱力ＤＲが、後記する図７に示す表の条件を満たしているか否かによって判定される。
【００５７】  ステップＳ２１０の結果、推定外乱力ＤＲの原因が路面傾斜によるものであると判定された場合（Ｓ２１０→Ｙｅｓ）、倒立制御部２４は、路面傾斜時のモータ３１Ｒへの指示トルクτｒＲを式（９）に基づいて算出し（Ｓ２１１）、図３のステップＳ１０７へリターンする。
【００５８】  ステップＳ２１０の結果、推定外乱力ＤＲの原因が路面傾斜によるものでないと判定された場合（Ｓ２１０→Ｎｏ）、倒立制御部２４は推定外乱力ＤＲの原因が柔らかい路面によるものであるか否かを判定する（Ｓ２１２）。推定外乱力ＤＲの原因が柔らかい路面であるか否かの判定は車輪角速度ｄφＲと推定外乱力ＤＲが、後記する図８の表の条件を満たしているか否かによって判定される。
【００５９】  ステップＳ２１２の結果、推定外乱力ＤＲの原因が柔らかい路面によるものであると判定された場合（Ｓ２１２→Ｙｅｓ）、倒立制御部２４は、柔らかい路面時のモータ３１Ｒへの指示トルクτｒＲを、以下の式（１１）基づいて算出し（Ｓ２１３）、図３のステップＳ１０７へリターンする。
【００６０】
0005873879.tif 000009
【００６１】  ステップＳ２１２の結果、推定外乱力ＤＲの原因が柔らかい路面によるものではないと判定された場合（Ｓ２１２→Ｎｏ）、倒立制御部２４は、モータ３１Ｒへの指示トルクτｒＲを式（９）に基づいて算出し（Ｓ２１４）、図３のステップＳ１０７へリターンする。
【００６２】  以上の処理によって、外乱力（推定外乱力）の種類が判別され、判別された外乱力に応じて指示トルクが算出される。図４の処理は右車輪４Ｒについてのものであるが、車輪角速度および推定外乱力を、左車輪４Ｌのものとして、同様の処理を右車輪４Ｒと並行して左車輪４Ｌについても行うことで、左車輪４Ｌに対する外乱原因の判別も行われる。  なお、図４の処理において、ステップＳ２０１，Ｓ２０３，Ｓ２０５，Ｓ２０７，Ｓ２１０，Ｓ２１２の各判定は、この順番に行われなくてもよい。
【００６３】  この後、機構制御部３２は、モータ３１Ｒ，３１Ｌの発生するトルクτＲ，τＬが、倒立制御部２４から送信された左右モータ３１Ｒ，３１Ｌへの指示トルクτｒＲ，τｒＬに追従するように制御する。また、機構制御部３２は、モータ３１Ｒ，３１Ｌから実際に発生したトルクτＲ，τＬと、モータ３１Ｒ，３１Ｌを介して得られる車輪角速度ｄφＲ，ｄφＬ及び車輪角度φＲ，φＬを取得し、倒立制御部２４へ送信する。
【００６４】  本実施形態によれば、外乱力（推定外乱力）の種類を判別し、その判別結果に応じた指示トルクを決定するので、様々な種類の外乱力に対応でき、安定した走行を可能とする倒立振子型移動装置１を提供することができる。
【００６５】［判別条件］  続いて、図５〜図８を参照して、各推定外乱力の種別の判別条件を説明する。ここでは、右車輪４Ｒについて説明するが、左車輪４Ｌについても同様の判別条件が用いられる。《スリップ：図４のステップＳ２０５》  図５は、スリップに対する判別条件を示す表である。  なお、図５〜図８において、車輪角速度が「＋」であるとは、倒立振子型移動装置１が前方に向うよう車輪４Ｒが回転していることを示し、「−」であるとは、倒立振子型移動装置１が後方に向うよう車輪４Ｒが回転していることを示す。  また、推定外乱力が「＋」であるとは、倒立振子型移動装置１が前方に向うように、車輪４Ｒに対して推定外乱力が余計に働いていることを示し、「−」であるとは、倒立振子型移動装置１が後方に向うように、車輪４Ｒに対して推定外乱力が余計に働いていることを示す。  なお、推定外乱力が「０」であるとは、余分な推定外乱力が働いていない状態を示す、
【００６６】  図５に示すように、最初、車輪角速度、推定外乱力がともに「＋」であった後、所定時間内に、両者とも「−」に変化した場合、倒立制御部２４は外乱原因の判別結果として「スリップ」を出力する。つまり、スリップを起こしたため、前方に進んでいた倒立振子型移動装置１の車輪４Ｒが、車輪角速度および推定外乱力ともに「＋」方向に余計な力が働いた後、バランスをとるため、倒立振子型移動装置１が逆方向に進もうとすると、ここでもスリップが生じて、車輪角速度および推定外乱力ともに「−」方向に余計な力が働いた場合、倒立制御部２４は外乱原因を「スリップ」と判別する。  なお、判別条件が生じる順番は逆でもよい。つまり、最初、車輪角速度および推定外乱力の両者が「−」であり、所定時間内に、車輪角速度および推定外乱力の両者が「＋」となった場合でも、倒立制御部２４は外乱原因を「スリップ」と判別する。
【００６７】《段差：図４のステップＳ２０７》（前方上り段差）  図６は、段差に対する判別条件を示す表である。  図６に示すように、現在（あるいは所定の時点）の車輪角速度が「＋」で、推定外乱力が「−」である状態から、所定時間内に車輪角速度が「−」、推定外乱力が「０」の状態となった場合、倒立制御部２４は外乱原因を「前方上り段差」と判別する。これは、倒立振子型移動装置１が段差にぶつかって、その段差を乗り越えている状態（車輪角速度「＋」、推定外乱力「−」）から、段差を乗り越えた後、バランスをとるため、倒立振子型移動装置１がやや後退した（車輪角速度「−」、推定外乱力「０」）ことを意味している。
【００６８】  なお、判別条件が生じる順番は逆でもよい。つまり、最初、車輪角速度が「−」で推定外乱力が「０」の状態から、所定時間内に、車輪角速度が「＋」になり、推定外乱力が「−」となった場合でも、倒立制御部２４は外乱原因を「前方上り段差」と判別する。これは、倒立振子型移動装置１が段差にぶつかった後、一旦後退してから、段差を乗り越えることを意味する。
【００６９】（前方下り段差）  また、現在（あるいは所定の時点）の車輪角速度および推定外乱力の両者が「＋」である状態から、所定時間内に車輪角速度が「−」、推定外乱力が「０」の状態となった場合、倒立制御部２４は外乱原因を「前方下り段差」と判別する。これは、前進している下り段差を降りている状態（車輪角速度「＋」、推定外乱力「＋」）の後、倒立振子型移動装置１が、やや後退した（車輪角速度「−」、推定外乱力「０」）ことを意味している。
【００７０】  なお、判別条件が生じる順番は逆でもよい。つまり、最初、車輪角速度が「−」で推定外乱力が「０」の状態から、所定時間内に、車輪角速度および推定外乱力の両者が「＋」となった場合でも、倒立制御部２４は外乱原因を「前方下り段差」と判別する。これは、倒立振子型移動装置１が段差に遭遇した時点で、一旦後退してから、段差を降りることを意味する。
【００７１】（後方上り段差）  さらに、現在（あるいは所定の時点）の車輪角速度が「−」で、推定外乱力が「＋」である状態から、所定時間内に車輪角速度が「＋」、推定外乱力が「０」の状態となった場合、倒立制御部２４は外乱原因を「後方上り段差」と判別する。これは、後進している倒立振子型移動装置１が段差にぶつかって、その段差を後ろ向きに乗り越えている状態（車輪角速度「−」、推定外乱力「＋」）から、段差を乗り越えた後、バランスをとるため、倒立振子型移動装置１がやや前進した（車輪角速度「＋」、推定外乱力「０」）ことを意味している。
【００７２】  なお、判別条件が生じる順番は逆でもよい。つまり、最初、車輪角速度が「＋」で推定外乱力が「０」の状態から、所定時間内に、車輪角速度が「−」になり、推定外乱力が「＋」となった場合でも、倒立制御部２４は外乱原因を「後方上り段差」と判別する。これは、倒立振子型移動装置１が後ろ向きに段差にぶつかった後、一旦前進してから、段差を乗り越えることを意味する。
【００７３】（後方下り段差）  さらに、現在（あるいは所定の時点）の車輪角速度および推定外乱力の両者が「−」である状態から、所定時間内に車輪角速度が「＋」、推定外乱力が「０」の状態となった場合、倒立制御部２４は外乱原因を「後方下り段差」と判別する。これは、後進している倒立振子型移動装置１が段差を降りている状態（車輪角速度「−」、推定外乱力「−」）の状態後、バランスをとるため、倒立振子型移動装置１がやや前進した（車輪角速度「＋」、推定外乱力「０」）ことを意味している。
【００７４】  なお、判別条件が生じる順番は逆でもよい。つまり、最初、車輪角速度が「＋」で推定外乱力が「０」の状態から、所定時間内に、車輪角速度が「−」になり、推定外乱力が「−」となった場合でも、倒立制御部２４は外乱原因を「後方下り段差」と判別する。これは、倒立振子型移動装置１が後ろ向きに段差に遭遇した後、一旦前進してから、段差を降りることを意味する。
【００７５】《斜面：図４のステップＳ２１０》（上り傾斜面）  図７は、斜面に対する判別条件を示す表である。  図７に示すように、現在（あるいは所定の時点）の車輪角速度が「＋」で、推定外乱力が「−」である状態から、所定時間内に車輪角速度および推定外乱力の両者が「−」の状態となった場合、倒立制御部２４は外乱原因を「上り傾斜面」と判別する。これは、倒立振子型移動装置１が、上り傾斜面を上った（車輪角速度「＋」、推定外乱力「−」）後、バランスをとるため、一旦、この斜面を下る（車輪角速度「−」、推定外乱力「−」）ことを意味している。
【００７６】  なお、判別条件が生じる順番は逆でもよい。つまり、最初、車輪角速度および推定外乱力の両者が「−」の状態から、所定時間内に、車輪角速度が「＋」になり、推定外乱力が「−」となった場合でも、倒立制御部２４は外乱原因を「上り傾斜面」と判別する。これは、倒立振子型移動装置１が上り傾斜面を上り始めた直後に、バランスをとるため、一旦傾斜面を少し下ってから、傾斜面を登ることを意味する。
【００７７】
（下り傾斜面）
また、現在（あるいは所定の時点）の車輪角速度および推定外乱力の両者が「＋」である状態から、所定時間内に車輪角速度が「−」、推定外乱力が「＋」の状態となった場合、倒立制御部２４は推定外乱力の原因を「下り傾斜面」と判別する。これは、前進している倒立振子型移動装置１が、下り傾斜面を下った（車輪角速度「＋」、推定外乱力「＋」）後、バランスをとるため、倒立振子型移動装置１が、一旦、この斜面を登る（車輪角速度「−」、推定外乱力「＋」）ことを意味している。
【００７８】  なお、判別条件が生じる順番は逆でもよい。つまり、最初、車輪角速度が「−」で推定外乱力が「＋」の状態から、所定時間内に、車輪角速度および推定外乱力の両者が「＋」となった場合でも、倒立制御部２４は外乱原因を「下り傾斜面」と判別する。これは、倒立振子型移動装置１が下り傾斜面を下り始めた直後に、バランスをとるため、一旦傾斜面を登ってから、傾斜面を下ることを意味する。
【００７９】《柔らかい路面：図４のステップＳ２１２》  図８は、柔らかい路面に対する判別条件を示す表である。  図８に示すように、路面が柔らかいとき、車輪４Ｒが路面に沈み込むので、倒立振子型移動装置１が前後に動いても、車輪４Ｒには抵抗力（つまり、車輪角速度とは逆方向の推定外乱力）が働く。従って、図８に示すように、前進しようとしているが、後ろ向きに抵抗（推定外乱力）が働いている状態（車輪角速度「＋」、推定外乱力「−」）から、所定時間内に後進しようとしているが、前向きに抵抗（推定外乱力）が働いている状態（車輪角速度「−」、推定外乱力「＋」）となったとき、倒立制御部２４は外乱原因を「柔らかい路面」と判別する。
【００８０】  この判別条件が生じる順番は逆でもよい。つまり、倒立振子型移動装置１が後進しようとしているが、前向きに抵抗（推定外乱力）が働いている状態（車輪角速度「−」、推定外乱力「＋」）状態から、所定時間内に、前進しようとしているが、後ろ向きに抵抗（推定外乱力）が働いている状態（車輪角速度「＋」、推定外乱力「−」）となったとき、倒立制御部２４は外乱原因を「柔らかい路面」と判別する。
【００８１】（別の実施形態）  なお、本実施形態では、行動決定部２１において移動目標や、経路が予めプログラムされているとしたが、人間がジョイスティックやコンソールなどを用いて、直接指定（操縦）してもよい。また、人間がリアルタイムに操縦を行う場合においては、行動決定部２１が生成する移動目標の中に移動先位置が含まれない。従って、経路計画部２２が不要となり、速度計画部２３は並進に関わる速度計画のみを計画し、旋回に関する計画は、常に操縦者によりリアルタイムで入力される旋回角速度を用いる。この場合、倒立制御部２４は、式（５）、式（６）によるτｓ，τｃの算出に関し、位置と旋回角度に関する項を「０」として扱うとよい。
【００８２】  また、図３における一連の処理において、倒立制御部２４は推定外乱力ＤＲ，ＤＬをそのまま用いているが、推定外乱力ＤＲ，ＤＬの値をローパスフィルタなどのデジタルフィルタを介して整形して用いてもよい。また車輪４Ｒ，４Ｌ自体の柔らかさに由来する特性を取り除くため、倒立制御部２４が事前に十分硬い路面上で車輪角速度ｄφＲ，ｄφＬの正負それぞれの場合において、推定外乱力ＤＲ，ＤＬが、どの様な値をとるかを測定し、その測定結果を図３の処理で得られた推定外乱力ＤＲ，ＤＬから、車輪角速度ｄφＲ，ｄφＬの正負それぞれの場合においてそれぞれ予め減じておいてもよい。
【００８３】  そして、倒立制御部２４は、図４の外乱原因の判別において閾値を用い、外乱力（推定外乱力）の絶対値が閾値より小さい場合は「０」として扱うようにしてもよい。
【００８４】  さらに、倒立制御部２４は、車輪角速度および車輪角度を、それぞれ倒立振子型移動装置１の移動速度と位置、旋回角速度で変換したものを代わりに用いてもよい。
【００８５】  さらに、倒立制御部２４は、段差、路面傾斜、柔らかい路面などの外乱力の種別を判別した際に、路面形状と位置情報を同時に記憶部（不図示）に記録し、次回以降の路面形状と位置情報の記憶部への追加時に、記録してある路面形状や、位置情報を用いて補正を行うなどの利用をしてもよい。
【００８６】  また、本実施形態では外乱力を外乱推定オブザーバによる推定外乱力としたが、これに限らず、センサによる実測の外乱力を使用してもよい。
【００８７】  本実施形態によれば、前記したように外乱力（推定外乱力）の種類を判別し、その判別結果に応じた指示トルクを決定するので、様々な種類の外乱力に対応でき、安定した走行を可能とする倒立振子型移動装置１を提供することができる。
【符号の説明】
【００８８】  １      倒立振子型移動装置  ２      計算部  ３      移動機構部  ４，４Ｌ，４Ｒ  車輪  ５      姿勢センサ  ２１    行動決定部  ２２    経路計画部  ２３    速度計画部  ２４    倒立制御部  ３１，３１Ｌ，３１Ｒ  モータ  ３２    機構制御部
【図１】
0005873879.tif 000011
【図２】
0005873879.tif 000012
【図３】
0005873879.tif 000013
【図４】
0005873879.tif 000014
【図５】
0005873879.tif 000015
【図６】
0005873879.tif 000016
【図７】
0005873879.tif 000017
【図８】
0005873879.tif 000018