JP5871925B2 - 速度計画装置を備えた倒立振子型移動体 - Google Patents

Description

本発明は、速度計画装置を備えた倒立振子型移動体に関する。

本技術分野に関する従来技術としては、特開２００７−１６８６０３号公報（特許文献１）がある。
この従来技術では、駆動輪と操作部と障害物検知部と駆動輪を駆動制御する制御部を備えた同軸二輪移動台車があり、この台車の移動中にユーザが台車の停止を指示した場合には操作部が、障害物の存在が検知された場合には障害物検知部が制御部に停止指示信号を出力するようになっている。

この制御部は、停止指示信号に基づき、現在の車速に−１を乗じた値を目標車速に設定すると共に、目標車速に応じた回転数で駆動輪を駆動させるようになっている。このため、車体に対して、進行方向と反対の方向に進むように力が加わることになり、停止時の制動距離が短くなる。また、目標車速は、車体が停止するまで逐次０に近づくように変化される。この制御部により車体の姿勢が制御されながら目標車速を変化させることができるため、安定した姿勢での車体の緊急停止が可能となっている。

また、特開２００７−３１９９９１号公報（特許文献２）がある。
この従来技術では、左右の車輪及びこの車輪を回転駆動させる走行モータを有する移動機構と、この移動機構に支持された上体と移動機構を制御する制御装置とを備えた移動ロボットが記載されている。

この制御装置は複数の時系列目標値を生成する走行目標生成部を有しており、この走行目標生成部は、移動ロボットの接地点より上にある移動ロボットの位置及び傾きの線形和である特徴点の時系列値を、前記複数の時系列目標値に対する移動条件を満たすように生成するようになっている。

特開２００７−１６８６０３号公報 特開２００７−３１９９９１号公報

倒立振子型移動体はその底面積が小さいことから、人間が搭乗する移動体として利用するケースや、サービスロボットなど物を運搬するための台車として利用するケースが考えられる。このように、倒立振子型移動体は人間と共存しながらあらゆる環境で活躍することが期待されている。

人間の近傍で動作する倒立振子型移動体は曲がり角からの飛び出しや接触、段差乗越え時の衝撃などによる姿勢の乱れに対して速やかに姿勢を回復させ、最短距離で停止して安全を確保することが重要である。

しかしながら特許文献１記載の技術では駆動輪の最大トルクが考慮されておらず、倒立振子型移動体が必ずしも最大加速度に達しないため、最短距離で止まることは難しい。また、過大な加速度が要求された場合は転倒に至る可能性もある。

また特許文献２記載の技術では最大トルクを考慮した移動計画を行っているが、姿勢の乱れた状態で加減速中からの計画を行うことができない。したがって、移動中の速度変更が難しく、緊急停止等の姿勢の乱れが予想されるような状況下では活用できない可能性がある。また、最大加速度を維持しない為、最短距離で止まることも難しい。

本発明の目的は、加減速中や段差を踏み越え中に姿勢が乱れたような状況からも、安全な緊急停止を含む速度変更を可能にする倒立振子型移動体を提供することにある。

本発明の倒立振子型移動体は、移動体本体に懸架されて床面を走行面とし、進行方向に垂直に備えられた一対の車輪と、この車輪を回転させる駆動機構と、前記移動体本体の重心位置と前記一対の前記車輪の車軸を結ぶ直線と鉛直方向のなす上体の傾斜角速度を計測する姿勢計測センサと、少なくとも前記上体の傾斜角度、前記上体の傾斜角速度、車輪角度及び車輪角速度の時系列データに基づき速度計画を生成する速度計画部と、前記速度計画に基づき前記駆動機構を制御することで前記移動体本体の倒立状態を維持する機構制御部とを備えた倒立振子型移動体であって、前記移動体を緊急停止する必要が生じた場合、前記速度計画部は、前記上体の傾斜角度及び前記上体の傾斜角速度が、前記一対の車輪を前記上体より進行方向に対して逆方向に動かす場合の前記倒立振子型移動体の回転中心をｑとしたとき、ｑ ^（２） ＝１／２ｋ（ｑ ^（３） ） ^２ で表される第１の減速曲線及び、ｑ ^（２） ＝−１／２ｋ（ｑ ^（３） ） ^２ ＋Ｃで表される第２の減速曲線に沿うよう前記速度計画を生成することを特徴とする。

また、本発明の倒立振子型移動体は、前記駆動機構は、前記一対の車輪をそれぞれ回転駆動する２つのモータを備え、前記移動体の移動中に緊急停止する必要が生じた際において、前記２つのモータに最大モータトルクを発生させた後に、上体の傾斜角速度及び上体の傾斜角度が前記第１の減速曲線及び第２の減速曲線に沿うよう前記速度計画を生成することを特徴とする。

また、本発明の倒立振子型移動体は、前記速度計画部が、上体の傾斜角速度が前記第１の減速曲線においてｑ ^（３） ＝０となるよう前記速度計画を生成することを特徴とする。

本発明によれば、加減速中や段差を踏み越え中に姿勢が乱れたような状況からも、安全な緊急停止を含む速度変更を可能にする倒立振子型移動体を提供できる。

本発明が対象とする倒立振子型移動体の概略構成図である。 本発明が対象とする倒立振子型移動体のシステム構成を表すフロー図である。 本発明の実施例に係る行動決定部の処理を説明するフロー図である。 本発明の実施例に係る行動決定部の障害物との接触判定を説明する図である。 本発明の実施例に係る速度計画部の処理を説明するフロー図である。 本発明の実施例に係る倒立振子型移動体のモデル化の例を示す図である。 本発明の実施例に係る速度計画部の緊急停止動作処理を表すフロー図である。 本発明の実施例に係る速度計画部の緊急停止動作処理を説明する図である。 本発明の実施例に係る効果のシミュレーション結果を表すグラフ図である。

以下、本発明の一実施例を図に沿って説明する。

本実施例の倒立振子型移動体１の機構構成を図１を参照しながら説明する。
図１(a)は本実施例の倒立振子型移動体１の機構構成を説明する側面図であり、図１(b)は倒立振子型移動体１の正面図である。

図１(a)(b)において、倒立振子型移動体１は大きく移動機構１０と上体２により構成される。移動機構１０は左右１対から成る同軸上の車輪１３R、Lと機構制御部１１とモータ12R、Lを備え、車輪１３R、Lは其々独立のモータ１２R、Lに接続されている。モータ１２R、Lは機構制御部１１に接続され、所定のトルクを発生させるように制御される。車輪１３R、Lの回転角度と回転角速度はモータ１２R、Lを介して機構制御部１１によって取得される。

上体２は移動機構１０の上部に備えられている。上体２は後述する演算を行うための計算機３と、周囲の状況を測定するための環境センサ４と、車輪１３R、Lを除く倒立振子型移動体１の重心位置と車軸を結んだ直線と鉛直方向のなす傾斜角度θ、傾斜角速度ｄθを測定するための姿勢センサ５を備えている。計算機３と移動機構１０とは情報交換ができる様に接続されている。

次に図２を用いて倒立振子型移動体１のシステム構成について述べる。
図２は本発明が対象とする倒立振子型移動体のシステム構成を表すフロー図である。

図２において、計算機３は障害物認識部２１、行動決定部２２、経路計画部２３、速度計画部２４、倒立制御部２５を備えている。計算機３は環境センサ４、姿勢センサ５、移動機構１０に接続されている。環境センサ４は倒立振子型移動体１外部の状況を観測するためのセンサであり、例えば、レーザスキャナを用いることで、平面または立体で周囲の物体までの距離を測定することができる。また、例えば、ステレオカメラを用いることでも同様に周囲の物体までの距離を認識することができる。姿勢センサ５は例えば、ジャイロセンサを用いることで構成され、これにより上体２の傾斜角度θや傾斜角速度ｄθを計測することができる。障害物認識部２１は環境センサ４から得られる周囲の物体までの距離を、姿勢センサ５より得られる傾斜角度θを用いてｃｏｓθを乗じて補正し、障害物の位置と速度と形状を算出する。

障害物の算出手法としては例えば以下の方法（例えば、特開２００８-６５７５５号公報の方法）がある。

この方法ではまず、ある時刻tにレーザスキャナから得られた距離値の角度に対する急激な変化点を検出し、連続する点の纏まりごとにデータ列を分割してセグメントとする。これによって、時間tにおける各セグメントの重心などの代表位置、形状などの特徴量を得る。次に、時刻ｔ＋Δｔで同様に計算し、各セグメントの特徴量を得る。ここで、時刻tで得られたセグメントの特徴量と、時刻ｔ＋Δｔで得られたセグメントの特徴量を比較し、特徴量の近いセグメント同士は同一の障害物であると認識し、代表位置の変化量から、障害物の速度を算出する。また同一の障害物と認識したデータ列より形状を、得られた形状に内接する円の中心座標を算出する。また、移動速度がほぼ０である障害物は静止障害物とみなし、レーザスキャナによる各データ点を幅０の障害物とみなす。また上記の処理はセグメントごとに実行されるため、複数の障害物に対しても認識できる。以上の処理を行うことにより障害物認識部２１は単数または複数の障害物の位置と速度と形状を算出する。

行動決定部２２は障害物認識部２１から取得する障害物の位置と速度と形状、及び姿勢センサ５から取得する傾斜角度θ、傾斜角速度ｄθ、経路計画部２３から取得する経路計画、速度計画部２４から取得する速度計画から倒立振子型移動体１の挙動を決定する。

以下、行動決定部２２での行動決定処理を図３、図４で説明する。
図３は本発明の実施例に係る行動決定部の処理を説明するフロー図である。
図４は本発明の実施例に係る行動決定部の障害物との接触判定を説明する図である。
図３において、S２２_１では行動決定部２２は予め設定されている、またはプログラムにより決定される、または人間によって入力される移動目標を取得する。移動目標は移動先位置ｐｒ、移動先旋回方向ωｒ、最大移動速度ｖｒ、最大モータトルクτｒを含む。

S２２_２において行動決定部２２は経路計画部２３に移動目標を検討命令と共に送信し、経路計画部２３及び速度計画部２４から経路計画と速度計画を取得する。経路計画部２３と速度計画部２４の内部処理については後述する。

S２２_３において行動決定部２２は姿勢センサ５より傾斜角度θ及び傾斜角速度ｄθを、倒立制御部２５から倒立振子型移動体１の現在位置pを速度計画部２４を介して取得する。

S２２_４において行動決定部２２は数式１を用いて倒立振子型移動体１の姿勢が崩れていないかを判断する。

ただし、θ１u、ｄθu、θ２uは倒立振子型移動体１の特性により決定される定数であり、θ１u>０、ｄθu>０、θ２u>０である。数式１が真である場合、処理をS２２_１０へ進め、それ以外の場合はS２２_５へ処理を進める。

S２２_５において行動決定部２２は障害物認識部２１より障害物の位置poと速度vo、形状を円で近似し半径rを取得する。

S２２_６において行動決定部２２はS２２_５で取得した障害物の情報とS２２_２で取得した経路計画及び速度計画から倒立振子型移動体１と障害物が接触するかを判定する。接触を判定するには、例えば、図４に示すように倒立振子型移動体１の時刻ｔにおける速度をv(t)、位置を経路計画に沿い、現在時刻からt0秒後までの速度の積分引数にする関数ｐ（∫ｖ（ｔ）ｄｔ)とし、障害物と同様に倒立振子型移動体１の形状を半径ｗの円とすると数式２により倒立振子型移動体１と障害物との接触を予測する。

数式２が偽の場合、倒立振子型移動体１と障害物との接触は無いと判定し行動決定部２２は処理をS２２_７に進める。数式２が真の場合、行動決定部２２は処理をS２２_１０に進める。

S２２_７において、行動決定部２２はS２２_１において移動目標を取得した後に移動命令を発行しているかを判定し、移動命令が発行されていない場合はS２２_８において行動決定部２２は経路計画部２３と速度計画部２４に対しS２２_２で発行した移動目標と移動命令を発信し、さらに処理をS２２_９へ進める。移動命令が発信済みの場合、行動決定部２２は処理をS２２_９に進める。
S２２_９において行動決定部２２はS２２_１で取得した移動目標とS２２_３で取得した現在位置ｐを数式３を用いて比較する。

ただし、εは予め設定された定数である。数式３が真の場合、行動決定部２２は移動決定処理を終了する。数式３が偽の場合、行動決定部２２は処理をS２２_３へ戻り繰り返す。

S２２_１0において、行動決定部２２は経路計画部２３に緊急停止命令を発行し処理を終了する。
経路計画部２３は行動決定部２２より移動目標を取得し、経路を生成する。経路生成方法には速度計画部２４を介し移動機構１０より取得した現在位置ｐ、移動先位置ｐｒを直線で結ぶ。または、移動先位置ｐｒと現在位置ｐにおいて目標とする旋回方向ωｒと現在の旋回方向ωが異なる場合は現在位置ｐと移動先位置ｐｒを滑らかに接続するベジェ曲線やスプライン曲線を経路としても良い。また、経路計画部は生成した経路情報を経路上にある点の位置座標データ群として、行動決定部２２と速度計画部２４に送信するとともに、行動決定部２２から取得した検討命令や移動命令も速度計画部２４に送信する。

速度計画部２４は行動決定部２２より移動目標を、経路計画部２３より経路計画を、姿勢センサ５より上体の傾斜角度θと傾斜角速度ｄθを、移動機構10より倒立制御部25を介して車輪速度ｄφを取得し速度計画を生成する。また生成した速度計画と取得した経路計画を倒立制御部２５に送信する。

以下、速度計画部２４での速度計画処理を図５のフローチャートを用いて述べる。また、図５フローチャート中の処理S２４_７の詳細については図７のフローチャートを用いて述べる。
図５は本発明の実施例に係る速度計画部の処理を説明するフロー図である。
図６は本発明の実施例に係る倒立振子型移動体のモデル化の例を示す図である。

図５のS２４_１において、速度計画部２４は行動決定部２２より移動目標と移動命令または検討命令を取得する。S２４_２において、速度計画部２４は経路計画部２３より経路情報を取得する。S２４_３において、速度計画部２４は取得した経路情報から移動距離lを算出する。移動距離lの算出は、経路情報をｎ点からなる位置座標データ群であるので、数式４に従って算出する。

ただしhi(i=１..n)は経路情報個々の点を表す。

S２４_４において、速度計画部２４は姿勢センサ５より上体の傾斜角度θと傾斜角速度ｄθを、倒立制御部２５より車輪角速度ｄφと現在位置を取得する。

S２４_５において速度計画部２４は行動決定部２２より緊急停止命令を取得しているかを判断する。緊急停止命令を取得していない場合は処理をS２４_６へ進め、取得している場合は処理をS２４-７に進める。

S２４_６において速度計画部２４は移動目標から抽出する最大移動速度ｖｒ、最大モータトルクτｒ、及び算出した移動距離ｌ、姿勢センサから取得した傾斜角度θ、傾斜角速度ｄθをもとに速度計画を生成する。速度計画は倒立振子型移動体１の運動方程式に従って生成され、運動方程式算出のためのモデルを図６に、算出された運動方程式を数式５に示す。

車輪１３の半径をR、質量をM０、慣性モーメントをJ０、倒立振子型移動体１の車輪以外の部分の質量をM１、慣性モーメントをJ１、重心と車輪１３の車軸までの距離をL、重心と車軸を含む直線からの車輪１３の回転角度をφ、回転角速度をｄφ、重力加速度をｇとしている。

数式５は図６に示したモデルよりラグランジュ方程式を介し、θ=０近傍で線形化している。また、式を簡便にするために定数項部分をS、T、U、Vで置き換えた。

さらに数式５に対して媒介変数ｑを用いることにより状態量である、傾斜角度θ、傾斜角速度ｄθ、車輪角度φ、車輪角速度ｄφ、モータトルクτの関係を数式６のように得る。

ただしｑ（ｉ） ［ｉ＝１、２、３］はｑのｉ階微分を表す。数式６より不動点ｑを設計することで全ての状態量を設計できる。倒立振子型移動体１は速度を変更する際に、その力学的特性から一旦、車輪１３を上体２より進みたい方向に対して逆方向に動かす必要がある。この時の倒立振子型移動体１全体の回転中心がｑである。ｑの設計方法として、移動開始と移動終了時の傾斜角度θと傾斜角速度ｄθを０と見なせる場合は特許文献２記載の技術が利用できる。この時ｑ（４） は０、ｋ、−ｋ（ｋ＞０）のいずれかの値をとり、移動開始時（時刻ｔｓ）と移動終了時（時刻ｔｅ）に状態が安定していることを条件に、ｑ（ｉ）（ｔｓ）＝ ｑ（ｉ）（ｔｅ）＝０ ［ｉ＝１、２、３］を導き、さらに移動距離ｌ、最大移動速度ｖｒ、最大モータトルクτｒを用いてｑ（４）を設定し、ｑ（４）を積分し数式６の関係で変換することで全ての状態量を設計している。傾斜角度θ、傾斜角速度ｄθ、車輪角度φ、車輪角速度ｄφの時系列に沿ったデータ列を速度計画として生成する。

S２４_８において、速度計画部２４は行動決定部２２より取得した命令が検討命令ならば処理をS２４_９へ、移動命令ならば処理をS２４_１０へ進める。
S２４_９において、速度計画部２４は行動決定部２２へ生成した速度計画を送信し終了する。
S２４_１０において、速度計画部２４は行動決定部２２と倒立制御部２５へ生成した速度計画を送信し終了する。

S２４_７において、速度計画部２４は現状の状態量を開始地点とする緊急停止動作を実行し終了する。この場合では移動開始時(時刻ts)と移動終了時(時刻te)の状態が安定ではないため特許文献２の方法は使えない。

そこで図７に示すフローチャートと図８を用いて停止計画を作成する。
図７は本発明の実施例に係る速度計画部の緊急停止動作処理を表すフロー図である。
図８は本発明の実施例に係る速度計画部の緊急停止動作処理を説明する図である。

図８は横軸にq^(２)、縦軸にq^(３)をとったものであり、これらは倒立振子型移動体１の加速度と加速度変化率の関係を表す。

図７、図８において、本実施例では車輪速度ｄφが正の場合から０に減速するまでを扱うが、符号を適切にすることにより車輪速度ｄφが負の場合からも０まで加速することができる。

S２４_７_１において、速度計画部２４はkを定数とした数式７で表される減速曲線１００と数式８であらわされる減速曲線１０１を算出する。

数式７及び８においてk(>０)は時不変で共通であり、q⁽⁴⁾の絶対値と等しく、例えば、最大モータトルクτｒと最大移動速度ｖｒと車輪１３の半径から求められるｄφと数式６の関係を用いることで算出する。q^(４) はトルクが計画で直接指定されていない場合はｋ、-k(k>0)のいずれかの値とする。また、数式6よりｋつまりq⁽⁴⁾の絶対値が決定されているならば最大モータトルクτｒは最大上体傾斜角度θの一次関数の関係である。これを利用して、要求するモータトルクの絶対値が最大となる、θの絶対値が最大となるq^(３)＝０の場合について前述の一次関数を解くことでCが算出できる。本減速曲線に沿った計画に追従することで、倒立振子型移動体１は任意の姿勢から、最大モータトルクでモータ１２を駆動した後でも、最大の減速を行い停止することができる。

S２４_７_２において、速度計画部２４は、姿勢センサ５より傾斜角度θと傾斜角速度ｄθを取得し、数式６の関係を用いてそれぞれq^(２)とq^(３) に変換し、図8における減速曲線１００の右にあるか左にあるかを判定する。右の場合は処理をS２４_７_３に進め、減速曲線１００の近傍にある場合はS２４_７_４に処理を進め、十分に左にある場合はS２４_７_５に進める。

S２４_７_３において、速度計画部２４は倒立制御部２５にモータトルクτがモータ最大トルクτｒとなるように指示を送り、S２４_７_２へ処理を戻す。S24_7_３の間は図8に示すように倒立振子型移動体1の状態は左下方向に動く。

S２４_７_４において、速度計画部２４は、姿勢センサ５と機構制御部１１より倒立制御部２５を介して車輪角速度ｄφ、傾斜角度θと傾斜角速度ｄθを取得し、数式６の関係を用いてこれら状態量を変換し、q⁽ⁱ⁾ [i=１、２、３]を生成する。得られたq⁽ⁱ⁾に対してq^(４)=kとし数式６の関係とq^(４)を積分することで減速曲線１００に沿った速度計画を生成し、傾斜角速度ｄθ= q⁽³⁾=０となる部分までを倒立制御部２５に送信する。傾斜角速度ｄθ= q⁽³⁾=０となった場合は処理をS24_7_7に進める。S24_7_４の間は図8に示すように倒立振子型移動体1の状態は減速曲線100に沿って動く。

S２４_７_５において、速度計画部２４はモータトルクτが０となるように倒立制御部２５に指示を送り、倒立振子型移動体１を転倒させ移動を終了させる。

S２４_７_６において、速度計画部２４は、S２４_７_４と同様に減速曲線１００に従って減速し、減速曲線１００と減速曲線１０１の交点でq^(４)=-kとして減速曲線101に沿ってさらに減速を続ける計画において、q^(２)=q^(３)=０となった場合において車輪角速度ｄφが０となるかを判定する。判定結果が偽の場合は判定結果が真となるまで目標傾斜角度、目標傾斜角速度を維持することで減速を続け、ループ処理を行う。判定結果が真の場合はS２４_７_７に処理を進め判定結果が真となった減速計画を倒立制御部２５へ送信し終了する。

倒立制御部２５は速度計画部２４から送られた計画またはトルク指令と移動機構１０から取得した車輪１３R、Lの回転角度φR、Lと回転角速度ｄφR、Lに基づいてモータ１２R、Lを制御するための指示を機構制御部１１に送信する。計画として受信している場合では計画の時系列に従い、現在時刻に対応する目標値を使用する。また、倒立振子型移動体１の現在位置ｐ(t0)を算出する。現在時刻ｔ０における位置の算出方法は数式９による。

ｔは時刻を表し車輪角度φR、Lと回転角速度ｄφR、Lはｔの関数で記述される。またｆは車輪１３Rと１３Lの間隔である。
速度計画部２４から計画が状態量を目標として取得された場合は数式１０に従った並進用モータトルクτｓを算出する。

ただし、K_(i) [i=１、２、３、４]はスカラー量であり、ヒューリスティックに、またLQRやＨ∞制御論により設計できる。またθr、ｄθr、φ、ｄφはそれぞれ上体角度目標値、上体角速度目標値、車輪角度目標値、車輪角速度目標値である。またφは車輪回転角度φR、Lの平均、ｄφは車輪回転角速度ｄφR、Lの平均である。緊急停止動作時のように車輪回転角度φｒが指定されない場合は車輪回転角度に関する部分は無視する。速度計画部２４より最大トルク指示、または転倒指示を受け取った場合は取得したモータトルク目標値をそのまま機構制御部１１に送信する。
経路計画に追従するための旋回モータトルクτｃの算出法について述べる。経路計画は前述のように経路上の点群データでありｈ[i]で表記する(i=1、2、…ｎ）。倒立制御部２５は現在位置に最も近い経路計画上の点ｈ[j]と点ｈ[j]までの距離Hを算出する。次に経路計画上の点ｈ[j]の前後の点ｈ[j+1]、ｈ[j-1]を用いて点ｈ[j]での目標旋回方向ωｒとその旋回半径ηを算出する。次に現在の目標移動速度ｖrを旋回半径ηで除算し目標旋回角速度ｄωｒを得る。以上を用いて数式１１に従い旋回用モータトルクτｃを算出する。

ただし、K_(i) [i=５、６、７]はスカラー量であり、ヒューリスティックに、または前述のような各種制御理論により設計できる。倒立制御部２５は機構制御部１１に対してモータ１２Lに出力させるためのトルクとしてτｓとτｃの和を、モータ１２Rに出力させるためのトルクとしてτｓとτcの差を送信する。
移動機構１０が停止命令を受信した場合は、機構制御部１１はモータ１２R、Lがトルクを出力しないように制御する。また車輪１３R、Lの角度と角速度をモータ１２R、Lを介して計測し、倒立制御部２５に送信する。

以上のように構成された倒立振子型移動体１は姿勢が乱れた状態からでも最大加速度＝最大モータトルクで停止することができる。また、途中で加速度が０にならない為に目標値に上体の傾斜角度と上体の傾斜角速度が０になる場面が計画終了の点以外に存在しない。本実施例で構成された倒立振子型移動体１の緊急停止動作実施時のシミュレーションによる挙動を図９に示す。

図９は本発明の実施例に係る効果のシミュレーション結果を表すグラフ図である。
図９において、倒立振子型移動体１が加速中に本実施例の緊急計画を行った際の例を実線で、従来例として、緊急停止計画を単に車輪角速度ｄφと傾斜角度θと傾斜角速度ｄθを０としたものを破線で表している。従来手法では急激な目標値変化に対応できず、転倒に至っているが、本実施例の手法では速度変更に成功し、停止に成功している。

実施例１では、前進移動中に速度を０とする緊急停止を示したが、後進移動中に速度を０とする場合、または任意の速度に現状の速度から変更する場合にも用いることができる。また、実施例１の例えば、S２４_７_２やS２４_７_６では取得した現在の情報をもとに処理を進めたが、計画開始時点での状況をもとにシミュレートを行い、タイミングを決定し、全ての計画を予め生成しても良い。

実施例１では、経路計画部２３において経路を生成していたが、行動決定部２２と同様に予め設定されている経路や、外部より直接経路情報を取得しても良い。また、経路計画を速度計画部２４に送信するに際し、経路計画情報を座標のデータで送信せず、時刻をパラメータとした関数や時刻と座標を組みとするデータ群で送信しても良い。

実施例１の緊急停止動作生成では最大加速度で停止するとしたが、変更後速度が変更前である現在の速度との差が小さい場合においては最大加速度を用いず、適当に小さな加速度を用いても良い。また、減速曲線に姿勢を動かすS２４_７_３においても最大トルクを用いなくとも良い。

実施例１では姿勢センサから上体の傾斜角度を取得するとしたが、姿勢センサは傾斜角速度のみを測定し、計算機が傾斜角速度を取得後に積分することで傾斜角度を得ても良い。

１・・・倒立振子型移動体、２・・・上体、３・・・計算機、４・・・環境センサ、５・・・姿勢センサ、１０・・・移動機構、１１・・・機構制御部、１２・・・モータ、１３・・・車輪。

Claims (3)

1. 移動体本体に懸架されて床面を走行面とし、進行方向に垂直な同一平面上に備えられた一対の車輪と、この車輪を回転させる駆動機構と、前記移動体本体の重心位置と前記一対の前記車輪の車軸を結ぶ直線と鉛直方向のなす上体の傾斜角速度を計測する姿勢計測センサと、少なくとも前記上体の傾斜角度、前記上体の傾斜角速度、車輪角度及び車輪角速度の時系列データに基づき速度計画を生成する速度計画部と、前記速度計画に基づき前記駆動機構を制御することで前記移動体本体の倒立状態を維持する機構制御部とを備えた倒立振子型移動体であって、
   前記移動体を緊急停止する必要が生じた場合、
   前記速度計画部は、前記上体の傾斜角度及び前記上体の傾斜角速度が、前記一対の車輪を前記上体より進行方向に対して逆方向に動かす場合の前記倒立振子型移動体の回転中心をｑとしたとき、ｑ ^（２） ＝１／２ｋ（ｑ ^（３） ） ^２ で表される第１の減速曲線及び、ｑ ^（２） ＝−１／２ｋ（ｑ ^（３） ） ^２ ＋Ｃで表される第２の減速曲線に沿うよう前記速度計画を生成することを特徴とする倒立振子型移動体。
2. 請求項１に記載の倒立振子型移動体において、
   前記駆動機構は、前記一対の車輪をそれぞれ回転駆動する２つのモータを備え、
   前記移動体の移動中に緊急停止する必要が生じた際において、前記２つのモータに最大モータトルクを発生させた後に、上体の傾斜角速度及び上体の傾斜角度が前記第１の減速曲線及び第２の減速曲線に沿うよう前記速度計画を生成することを特徴とする倒立振子型移動体。
3. 請求項１または請求項２に記載の倒立振子型移動体において、
   前記速度計画部は、上体の傾斜角速度が前記第１の減速曲線においてｑ ^（３） ＝０となるよう前記速度計画を生成することを特徴とする倒立振子型移動体。

Images