JP5336955B2 - 倒立振子型移動体及びその制御方法 - Google Patents

Description

本発明は、倒立振子型の移動体に関する。特に、倒立振子型の移動体が設定された目標経路上を精度よく走行するための技術に関する。
本明細書中、「移動体の進行方向」とは、移動体が移動する平面において、その平面に投影した車輪の回転軸に対して直交する方向をいう。「移動体の旋回方向」とは、移動体の鉛直軸回りの方向をいう。また、「目標軌道」とは、移動体の経時的な目標位置（すなわち、移動体の目標位置（目標経路）を時間の関数として表したもの）をいう。

倒立振子型の移動体は、車輪と、その車輪に支持される車体を備えている。この移動体では、車体の重心が車輪の回転中心よりも上方に位置し、車輪を回転駆動しないと車体が倒立状態を維持できない。このため、この種の移動体では、車輪を回転駆動することで、車体を倒立状態に維持しながら路面上を走行する。この種の移動体の従来技術としては、例えば、特許文献１，２に開示されたものが知られている。

特開２００４−２９５４３０号公報 特開２００７−３１９９９１号公報

倒立振子型の移動体では、移動体の進行方向の速度応答性が、移動体の旋回方向の速度応答性に対して遅いという構造的な特性を有している。詳細に説明すると、図１２に示すように、倒立振子型の移動体では、車体が倒立状態を維持するために、車輪１００と路面Ｒの接触点Ａまわりのモーメントが０とならなければならない。すなわち、移動体の進行方向の慣性力ｍａによる接触点Ａまわりのモーメントと、移動体に作用する重力ｍｇによる接触点Ａまわりのモーメントが０とならなければならない。このため、移動体の進行方向の加速度は、車体の傾斜角の関数となり、突変させることが不可能となる。一方、移動体の旋回方向の加速度については上述した問題はなく、事実上、ステップ状に加速度を変化させることが可能となる。従って、倒立振子型の移動体では、移動体の進行方向の速度応答性が、移動体の旋回方向の速度応答性に対して遅いという特性を有している。かかる特性を無視して目標軌道を設定すると、車輪移動の持つ非ホロノミック拘束により移動体が目標軌道からずれ、目標の経路を走行しないという問題が生じることがある。

本発明は、上記した実情に鑑みてなされたものであり、倒立振子型の移動体の特性を考慮することなく目標軌道を設定しても、目標経路上を精度良く走行することができる倒立振子型の移動体を提供する。

本明細書に開示する倒立振子型の移動体は、車輪と、車輪に支持される車体を備えており、車輪を回転駆動することで車体を倒立状態に維持しながら路面上を走行する。この移動体は、第１制御指令値算出手段と第２制御指令値算出手段と駆動手段を備えている。第１制御指令値算出手段は、移動体の目標位置を時間の関数で表した目標軌道を用いて、車体が倒立を維持しつつ移動体の進行方向の位置を制御するための第１制御指令値を算出する。第２制御指令値算出手段は、目標軌道と実測又は推定される移動体の進行方向の位置に関する情報とを用いて、移動体の旋回方向の角度を制御するための第２制御指令値を算出する。駆動手段は、第１制御指令値と第２制御指令値に基づいて車輪を駆動する。
この移動体では、第１制御指令値算出手段が進行方向の位置を制御するための第１制御指令値を算出し、第２制御指令値算出手段が旋回方向の角度を制御するための第２制御指令値を算出し、これらの制御指令値に基づいて車輪が駆動される。ここで、第２制御指令値算出手段は、目標軌道と移動体の進行方向の位置に関する情報を用いて第２制御指令値を算出する。このため、移動体の進行方向の応答速度の遅れによって移動体が目標軌道からずれた場合は、そのずれを修正して旋回方向の第２制御指令値を算出することができる。これによって、移動体は目標経路上を精度よく走行することができる。
ここで、「移動体の進行方向の位置に関する情報」とは、移動体の進行方向の位置（移動距離）を特定することができる情報であり、例えば、移動体の進行方向の加速度、速度等の物理量が相当する。

上記移動体では、第１制御指令値算出手段と第２制御指令値算出手段と駆動手段とを所定周期毎に繰り返し実行することで、移動体が路面上を走行することが好ましい。
この構成によると、所定周期毎に制御指令値が算出され、目標経路からのずれが所定周期毎に修正される。このため、移動体は目標経路上を精度よく走行することができる。

上記移動体の一の態様では、移動体の進行方向の位置に関する情報を測定するためのセンサ（例えば、車輪の回転角速度を検出するエンコーダ等）をさらに備えることができる。第２制御指令値算出手段は、センサの検出結果から移動体の経路長さを算出し、その算出された経路長さに対応する目標軌道上の位置を算出し、その算出された位置を用いて移動体の旋回方向の角度を制御するための第２制御指令値を算出してもよい。
この構成によると、測定された進行方向の位置に関する情報を利用して旋回方向の第２制御指令値が算出される。実測された位置に関する情報を利用することで、目標経路に沿った（目標経路に近い又は目標経路の形状に沿った）動きが実現でき、移動体は目標経路上を精度よく移動することができる。

上記移動体の他の態様では、第２制御指令値算出手段は、「目標軌道」と「第１制御指令値算出手段で算出された第１制御指令値から推定される移動体の進行方向の位置に関する情報」とから算出される「その推定された進行方向の位置に関する情報に対応した移動体の目標位置」に基づいて、移動体の旋回方向の角度を制御するための第２制御指令値を算出することが好ましい。
この構成によると、第１制御指令値から推定される移動体の進行方向の位置に関する情報を利用して第２制御指令値が算出される。第１制御指令値から推定される進行方向の位置に関する情報を利用することで、移動体の目標経路からのずれを見込んで旋回方向の制御指令値を算出することができる。この構成によっても、移動体は目標経路上を精度よく移動することができる。

上記移動体の他の態様では、移動体の位置と旋回方向の角度を測定するセンサと、センサの検出結果と目標軌道から得られる（１）移動体の進行方向の位置偏差と（２）移動体の進行方向に直交する方向の位置偏差と（３）移動体の旋回方向の角度偏差とに基づいて、移動体の進行方向の位置に関する目標値と、移動体の旋回方向の角度に関する目標値を算出するトラッキングコントローラをさらに備えることができる。そして、第１制御指令値算出手段は、トラッキングコントローラで算出された移動体の進行方向の位置に関する目標値に基づいて第１制御指令値を算出し、第２制御指令値算出手段は、トラッキングコントローラで算出された移動体の旋回方向の角度に関する目標値と算出された第１制御指令値に基づいて、第２制御指令値を算出することが好ましい。
この構成によると、センサの検出結果から移動体の位置及び旋回方向の角度（すなわち、向き（姿勢））が分かり、これらを用いて目標軌道からの偏差が特定され、この特定された偏差を用いて移動体の進行方向及び旋回方向の各目標値が算出される。そして、これらの目標値に基づいて、第１制御指令値と第２制御指令値を算出する。その際、第１制御指令値を利用して第２制御指令値を算出するため、進行方向の遅れを考慮して旋回方向の第２制御指令値が算出される。この構成によっても、移動体は目標経路上を精度よく移動することができる。

また、本明細書は、目標経路を精度よく走行することができる倒立振子型の移動体の制御方法を開示する。すなわち、この制御方法は、車輪と、車輪に支持される車体と、車輪を回転駆動する駆動装置と、駆動装置を制御する制御装置を備えており、駆動装置によって車輪を回転駆動することで車体を倒立状態に維持しながら路面上を走行する倒立振子型移動体の制御方法である。
この制御方法では、（１）移動体の目標位置を時間の関数で表した目標軌道を用いて、車体が倒立を維持しつつ移動体の進行方向の位置を制御するための第１制御指令値を算出する工程と、（２）目標軌道と実測又は推定される移動体の進行方向の位置に関する情報とを用いて、移動体の旋回方向の角度を制御するための第２制御指令値を算出する工程と、（３）第１制御指令値と第２制御指令値に基づいて車輪を駆動する工程と、を有している。そして、上記各工程を繰返し実行することで移動体を制御する。
この制御方法によっても、移動体の進行方向の位置に関する情報を利用して移動体の旋回方向の制御指令値を算出するため、目標経路との誤差を小さくすることができる。

移動体の斜視図。 移動体の進行方向と旋回方向を説明するための図。 制御装置３０の機能ブロック図。 進行方向目標値算出部４０の機能ブロック図。 旋回方向目標値算出部５０の機能ブロック図。 制御装置３０の処理を示すフローチャート。 第１実施例の移動体の動作を説明するための図。 第２実施例に係る制御装置６０の機能ブロック図。 制御装置６０の処理を示すフローチャート。 第３実施例に係る制御装置８８の機能ブロック図。 制御装置８８の処理を示すフローチャート。 倒立振子型の移動体の構造的な特性を説明するための図。

下記に詳細に説明する実施例の主要な特徴を最初に列記する。
（形態１）移動体は、同一の車軸上に配された２つの車輪と、車輪によって支持される車体によって構成される。
（形態２）車体は、各車輪を駆動するモータと、車体の傾斜角速度を検出するジャイロセンサと、各モータの回転角速度を検出するエンコーダと、これらのセンサの検出結果に応じてモータを駆動する制御装置を備えている。
（形態３）制御装置は、目標軌道（ｘ（ｔ），ｙ（ｔ））を用いて、移動体の進行方向の位置を制御するための制御指令値を算出する。
（形態４）制御装置は、車体の進行方向の位置と速度と加速度の少なくとも一つを用いて、目標軌道（ｘ（ｔ），ｙ（ｔ））を修正する。そして、修正した目標軌道（ｘ’，ｙ’）を用いて、移動体の旋回方向の角度を制御する制御指令値を算出する。
（形態５）制御装置は、エンコーダにより検出されたモータの回転角速度を用いて、直前の制御周期から移動体が移動した距離を算出する。次に、算出された移動距離と目標軌道（ｘ（ｔ），ｙ（ｔ））を用いて、その移動距離を実現すべきだった時刻ｔ’を算出する。そして、時刻ｔ’における目標軌道（ｘ（ｔ’），ｙ（ｔ’））を用いて、移動体の旋回方向の角度を制御するための制御指令値を算出する。
（形態６）制御装置は、今回の制御周期で算出された「移動体の進行方向の位置を制御するための制御指令値」を用いて、今回の制御周期において移動体が移動するであろう距離を算出する。次に、算出された移動距離と目標軌道（ｘ（ｔ），ｙ（ｔ））を用いて、その移動距離を実現すべき時刻ｔ’を算出する。そして、時刻ｔ’における目標軌道（ｘ（ｔ’），ｙ（ｔ’））を用いて、移動体の旋回方向の角度を制御するための制御指令値を算出する。
（形態７）移動体は、移動体の位置と姿勢（ｘ，ｙ，φ）を測定するセンサを備える。この種のセンサとしては、ＧＰＳセンサ、レーザレンジセンサ、モーションキャプチャ等を用いることができる。
（形態８）制御装置は、測定した移動体の位置と姿勢（ｘ，ｙ，φ）と、目標軌道（ｘ（ｔ），ｙ（ｔ））とから、（１）移動体の進行方向の位置偏差と（２）移動体の進行方向に直交する方向の位置偏差と（３）移動体の姿勢偏差を算出する。次に、算出されたこれらの偏差を用いて、移動体の進行方向の位置に関する目標値（例えば、進行方向の速度）と、移動体の旋回方向の角度に関する目標値（例えば、旋回方向の角速度）を算出する。そして、移動体の進行方向の位置を制御するための制御指令値については、算出された進行方向の位置に関する目標値を用いて算出する。移動体の旋回方向の角度を制御するための制御指令値については、算出された進行方向の位置に関する目標値と、算出された移動体の旋回方向の角度に関する目標値と、算出された移動体の進行方向の位置を制御するための制御指令値を用いて算出する。
（形態９）形態８において、移動体の旋回方向の角度を制御するための制御指令値は、「移動体の旋回方向の角度に関する目標値」に、「移動体の進行方向の位置に関する目標値」と「移動体の進行方向の位置を制御するための制御指令値」の比（すなわち、「移動体の進行方向の位置を制御するための制御指令値」／「移動体の進行方向の位置に関する目標値」）を掛けて算出された「移動体の旋回方向の角度に関する新しい目標値」に基づいて算出される。

（第１実施例） 本発明の一実施例に係る倒立振子型の移動体について、図面を参照しながら説明する。図１に示すように、移動体１０は、車軸１６上に配された２つの車輪１２、１４と、車輪１２、１４によって車軸１６周りに傾動可能に支持された車体２０によって構成される。車体２０の重心は、車軸１６よりも上方に位置している。移動体１０は、車輪１２、１４を駆動することによって車体２０の倒立状態を維持し、かつ、車輪１２、１４を駆動することによって床面を走行する。

車輪１２は、モータ２２と接続されており、モータ２２によって駆動される。車輪１４は、モータ２４と接続されており、モータ２４によって駆動される。すなわち、車輪１２、１４はそれぞれ独立して駆動される。車体２０の倒立状態は、車体２０の傾斜角θ（車体２０の重心から車軸１６に下ろした垂線と、鉛直軸とが成す角度）に応じて車輪１２、１４を駆動することによって維持される。また、車輪１２、１４を駆動することによって、移動体１０は床面を走行する。移動体１０の旋回方向（鉛直軸回りの方向）の角度φは、車輪１２、１４のそれぞれの回転角速度を制御することによって変更することができる。すなわち、移動体１０の床面（ｘ−ｙ平面）上の運動は、その非ホロノミック拘束によって、移動体１０の進行方向（車軸１６に対して直交する方向）の速度ｒと、移動体１０の旋回方向（鉛直軸回りの方向）の角速度φ’で表される（図２参照）。

車体２０は、車輪１２、１４を駆動するモータ２２、２４と、車体２０の傾斜角速度θ’を検出するジャイロセンサ２８と、モータ２２、２４に制御指令値を出力する制御装置３０を備えている。

車体２０内に配されたモータ２２、２４は、車輪１２、１４に連結されている。また、モータ２２、２４は制御装置３０と電気的に接続されている。モータ２２、２４は、制御装置３０から入力される制御指令値にしたがって、車輪１２、１４を駆動する。
モータ２２にはエンコーダ２２ａが設けられている。エンコーダ２２ａは、制御装置３０と電気的に接続されている。エンコーダ２２ａは、モータ２２の回転角速度（すなわち、車輪１２の回転角速度）を検出する。エンコーダ２２ａが検出した車輪１２の回転角速度は、制御装置３０に入力される。また、モータ２４にもエンコーダ２４ａが設けられている。エンコーダ２４ａは、車輪１４の回転角速度を検出する。エンコーダ２４ａが検出した車輪１４の回転角速度も、制御装置３０に入力される。

ジャイロセンサ２８は、車体２０の内部に配されている。ジャイロセンサ２８は制御装置３０と電気的に接続されている。ジャイロセンサ２８は、車体２０の傾斜角速度θ’を検出する。検出した傾斜角速度θ’は、制御装置３０に入力される。

制御装置３０は、ＣＰＵ，ＲＯＭ，ＲＡＭを備えたマイクロプロセッサによって構成されている。制御装置３０は、車体２０内に配されている。制御装置３０は、モータ２２、２４と、エンコーダ２２ａ、２４ａと、ジャイロセンサ２８に電気的に接続されている。
図３は制御装置３０の機能を示すブロック図である。図３に示すように、制御装置３０は、目標軌道入力部３８、進行方向目標値算出部４０、倒立制御コントローラ３２、旋回方向目標値算出部５０、旋回制御コントローラ３４、指令値合成部３６によって構成されている。なお、図３においては、図の見易さを考慮してモータ及びエンコーダを１つだけ図示したが、実際には２つのモータ２２、２４にそれぞれ独立して制御指令値が出力される。

目標軌道入力部３８は、移動体１０の目標軌道（ｘ（ｔ），ｙ（ｔ））を記憶する。目標軌道入力部３８に記憶されている目標軌道（ｘ（ｔ），ｙ（ｔ））は、進行方向目標値算出部４０と旋回方向目標値算出部５０に入力される。目標軌道（ｘ（ｔ），ｙ（ｔ））は、移動体１０の床面上の位置（ｘ，ｙ）を時間ｔの関数で表したものである。目標軌道（ｘ（ｔ），ｙ（ｔ））は、外部装置（例えば、遠隔操縦装置又はあらかじめ決められた起動を生成する装置など）を用いて、目標軌道入力部３８に入力される。

進行方向目標値算出部４０は、制御周期毎に、目標軌道（ｘ（ｔ），ｙ（ｔ））を用いて、移動体１０の進行方向の制御目標値（目標接線速度ｓ’（ｔ）、目標経路長ｓ（ｔ））を算出する。進行方向目標値算出部４０の詳細については、後で詳述する。

倒立制御コントローラ３２は、制御周期毎に、ジャイロセンサ２８で検出された車体２０の傾斜角速度θ’と、エンコーダ２２ａ，２４ａで検出された車輪１２，１４の回転角速度と、進行方向目標値算出部４０で算出された目標経路長ｓ（ｔ）及び目標接線速度ｓ’（ｔ）に基づいて、移動体１０が倒立を維持し、かつ、移動体１０の進行方向の位置を制御するための制御指令値を算出する。具体的には、エンコーダ２２ａ，２４ａで検出された車輪１２，１４の回転角速度と車体の傾斜角（ジャイロセンサ２８で検出）から算出される移動体の経路長ｓ_ｒ（ｔ）と目標経路長ｓ（ｔ）との偏差を小さくし、エンコーダ２２ａ，２４ａで検出された車輪１２，１４の回転角速度から算出される移動体の接線速度ｓ_ｒ’（ｔ）と目標接線速度ｓ’（ｔ）との偏差を小さくし、かつ、ジャイロセンサ２８で検出された車体２０の傾斜角速度θ’に基づいて移動体１０が倒立を維持するように、各モータ２２，２４を駆動するためのトルク指令値を算出する。倒立制御コントローラ３２の具体的な構成としては、例えば、特許文献１に開示された構成を用いることができる。

旋回方向目標値算出部５０は、制御周期毎に、目標軌道（ｘ（ｔ），ｙ（ｔ））と、エンコーダ２２ａ，２４ａで検出された車輪１２，１４の回転角速度を用いて、移動体１０の旋回方向の制御目標値（目標角度φ、目標角速度φ’）を算出する。

旋回制御コントローラ３４は、制御周期毎に、エンコーダ２２ａ，２４ａで検出された車輪１２，１４の回転角速度と、旋回方向目標値算出部５０で算出された目標角度φ及び目標角速度φ’に基づいて、移動体１０の旋回方向の位置を制御するための制御指令値を算出する。具体的には、「エンコーダ２２ａ，２４ａで検出された車輪１２，１４の回転角速度から算出される移動体１０の現在の角度φ_ｒ」と目標角度φとの偏差と、「エンコーダ２２ａ，２４ａで検出された車輪１２，１４の回転角速度から算出される移動体の角速度φ_ｒ’」と目標角速度φ’との偏差を小さくするように、各モータ２２，２４を駆動するためのトルク指令値を算出する。なお、旋回制御コントローラ３４の具体的な構成としては、例えば、特許文献１に開示された構成を用いることができる。

指令値合成部３６は、倒立制御コントローラ３２から出力されるトルク指令値と、旋回制御コントローラ３４から出力されるトルク指令値を合成し、その合成したトルク指令値をモータ２２，２４に出力する。モータ２２，２４は、この合成したトルク指令値に基づいて駆動される。

次に、上述した進行方向目標値算出部４０の詳細な構成について説明する。図４は進行方向目標値算出部４０の機能を示すブロック図である。図４に示すように、進行方向目標値算出部４０は、目標速度算出部４２と目標接線速度算出部４４と目標経路長算出部４６によって構成される。
目標速度算出部４２は、目標軌道入力部３８から入力された目標軌道（ｘ（ｔ），ｙ（ｔ））から移動体１０の目標速度を算出する。具体的には、目標軌道（ｘ（ｔ），ｙ（ｔ））を微分することによって、目標速度（ｘ’（ｔ），ｙ’（ｔ））を算出する。算出された目標速度（ｘ’（ｔ），ｙ’（ｔ））は目標接線速度算出部４４に入力される。
目標接線速度算出部４４は、入力された目標速度（ｘ’（ｔ），ｙ’（ｔ））から目標接線速度を算出する。目標接線速度とは、ベクトルで与えられた目標速度（ｘ’（ｔ），ｙ’（ｔ））の絶対値（スカラー）である。したがって、目標接線速度ｓ’（ｔ）は、（ｘ’（ｔ）^２＋ｙ’（ｔ）^２）^１／２によって算出される。算出された目標接線速度ｓ’（ｔ）は、倒立制御コントローラ３２と目標経路長算出部４６に入力される。
目標経路長算出部４６は、入力された目標接線速度（ｘ’（ｔ）^２＋ｙ’（ｔ）^２）^１／２を積分することによって移動体１０の目標経路長ｓ（ｔ）を算出する。算出された目標経路長ｓ（ｔ）は、倒立制御コントローラ３２と目標経路長算出部４６に入力される。
上述したことから明らかなように、移動体１０の進行方向の目標値（目標接線速度ｓ’、目標経路長ｓ）は目標軌道（ｘ（ｔ），ｙ（ｔ））のみに基づいて算出される。

次に、上述した旋回方向目標値算出部５０の詳細な構成について説明する。図５は旋回方向目標値算出部５０の機能を示すブロック図である。図５に示すように、旋回方向目標値算出部５０は、計測接線速度算出部５９と、計測経路長算出部５８と、修正時刻算出部５７と、新目標位置算出部５１と、新目標速度算出部５２と、新目標加速度算出部５４と、目標姿勢（角度）算出部５３と、新目標曲率算出部５５と、目標角速度算出部５６によって構成される。

計測接線速度算出部５９と計測経路長算出部５８と修正時刻算出部５７と新目標位置算出部５１は、エンコーダ２２ａ，２４ａで検出された車輪１２，１４の回転角速度を用いて、進行方向の応答性の遅れを考慮した新しい目標位置（ｘ，ｙ）を算出する。
すなわち、計測接線速度算出部５９は、エンコーダ２２ａ，２４ａで検出された車輪１２，１４の回転角速度から、移動体１０の接線速度ｓ_ｒ’を算出する。すなわち、車輪１２，１４の回転角速度と、車輪１２，１４の幾何学的関係（車輪１２，１４の直径，車輪１２，１４間の距離）とから、移動体１０の接線速度ｓ_ｒ’を算出する。算出された接線速度ｓ_ｒ’は、目標角度算出部５６と計測経路長算出部５８に入力される。
計測経路長算出部５８は、計測接線速度算出部５９で算出された接線速度ｓ_ｒ’を積分することによって、移動体１０の経路長ｓ_ｒを算出する。算出された経路長ｓ_ｒは、修正時刻算出部５７に入力される。
修正時刻算出部５７は、算出された経路長ｓ_ｒと目標軌道（ｘ（ｔ），ｙ（ｔ））とから、その算出された経路長ｓ_ｒを実現すべきであった時刻ｔ’を算出する。すなわち、目標軌道（ｘ（ｔ），ｙ（ｔ））から時間ｔと目標経路長ｓ（ｔ）の関係が得られる。次いで、時間ｔと目標経路長ｓ（ｔ）の関係に基づいて、計測経路長算出部５８で算出された経路長ｓ_ｒとなるときの時刻ｔ’を算出する。算出された時刻ｔ’は新目標位置算出部５１に入力される。
新目標位置算出部５１は、修正時刻算出部５７で算出した時刻ｔ’を用いて、新しい目標位置（ｘ（ｔ’），ｙ（ｔ’））を算出する。したがって、進行方向の速度応答性の遅れから、現実の経路長ｓ_ｒが目標軌道から得られる目標経路長ｓ（ｔ）とならない場合には、経路長ｓ_ｒを実現すべきであった時刻ｔ’が求められ、その時刻ｔ’における目標位置が新しい目標位置（ｘ（ｔ’），ｙ（ｔ’））として採用される。

新目標速度算出部５２と目標姿勢算出部５３と新目標加速度算出部５４と新目標曲率算出部５５と目標角速度算出部５６は、上記のように算出された新しい目標位置（ｘ（ｔ’），ｙ（ｔ’））に基づいて、移動体１０の旋回方向の制御目標値（目標角度φ、目標角速度φ’）を算出する。
すなわち、新目標速度算出部５２は、新しく算出された新目標位置（ｘ（ｔ’），ｙ（ｔ’））を用いて、移動体１０の新目標速度を算出する。具体的には、新目標位置（ｘ（ｔ’），ｙ（ｔ’））を微分することによって、新目標速度（ｘ’（ｔ’），ｙ’（ｔ’））を算出する。算出された新目標速度（ｘ’（ｔ’），ｙ’（ｔ’））は目標姿勢算出部５３と新目標加速度算出部４４と新目標曲率算出部５５に入力される。
目標姿勢算出部５３は、算出された新目標速度（ｘ’（ｔ’），ｙ’（ｔ’））を用いて、移動体１０の目標姿勢（すなわち、旋回方向の目標角度φ）を算出する。すなわち、ｔａｎφ＝［ｙ’（ｔ’）］／［ｘ’（ｔ’）］の関係が成立するため、この式に算出された新目標速度（ｘ’（ｔ’），ｙ’（ｔ’））を代入することで、目標角度φが算出される。算出された目標角度φは、旋回制御コントローラ３４に入力される。
新目標加速度算出部５４は、算出された新目標速度（ｘ’（ｔ’），ｙ’（ｔ’））を用いて、移動体１０の新目標加速度を算出する。具体的には、新目標速度（ｘ’（ｔ’），ｙ’（ｔ’））を微分することによって、新目標加速度（ｘ’’（ｔ’），ｙ’’（ｔ’））を算出する。算出された新目標加速度（ｘ’’（ｔ’），ｙ’’（ｔ’））は、新目標曲率算出部５５に入力される。
新目標曲率算出部５５は、新目標速度（ｘ’（ｔ’），ｙ’（ｔ’））と、新目標加速度（ｘ’’（ｔ’），ｙ’’（ｔ’））を用いて、移動体１０の新目標曲率１／Ｒを算出する。具体的には、新目標速度（ｘ’（ｔ’），ｙ’（ｔ’））から移動体の目標接線速度ｖ’を算出し、新目標加速度（ｘ’’（ｔ’），ｙ’’（ｔ’））から移動体の法線方向の目標加速度ａ_ｎを算出する。ここで、目標接線速度ｖ’と法線方向の加速度ａ_ｎと曲率半径Ｒの間には、ａ_ｎ＝ｖ’^２／Ｒの関係が成立する。このため、目標接線速度ｖ’と法線方向の加速度ａ_ｎが分かると、曲率半径Ｒを算出することができる（すなわち、曲率１／Ｒを算出することができる）。算出された曲率１／Ｒは目標角度算出部５６に入力される。
目標角速度算出部５６は、計測接線速度算出部５９で算出された現実の接線速度ｓ_ｒ’と、算出された曲率１／Ｒを用いて、移動体１０の旋回方向の目標角速度φ’を算出する。具体的には、接線速度ｓ_ｒ’に曲率（１／Ｒ）を掛けることで目標角速度φ’を算出する。算出された目標角速度φ’は旋回制御コントローラ３４に入力される。

なお、上述した実施例では、移動体１０の旋回方向の位置を制御するために、移動体１０の旋回方向の目標角度φ及び目標角速度φ’を用いた。しかしながら、目標角度φと目標角速度φ’の両者を常に使う必要はなく、例えば、移動体１０の旋回方向の目標角度φにのみ基づいて、移動体１０の旋回方向の位置を制御するようにしてもよい。この場合は、上述した新目標加速度算出部５４と新目標曲率算出部５５と目標角速度算出部５６を無くすことができ、旋回方向目標値算出部５０の構成を簡略化することができる。

次に、上述のように構成される制御装置３０によって行われる処理について説明する。図６は制御装置３０の処理手順を示すフローチャートである。
図６に示すように、制御装置３０は、まず、制御装置３０が起動されてからの時間ｔ（すなわち、移動体１０の制御開始時からの経過時間）を読み込む（Ｓ２）。時間ｔは、例えば、制御装置３０にタイマを装備し、このタイマによって制御開始時からの経過時間を計測することで得られる。
次に、各モータ２２，２４のエンコーダ２２ａ，２４ａの値（すなわち、車輪１２，１４の回転角速度）を読込む（Ｓ４）。
次に、ステップＳ４で読込んだエンコーダ２２ａ，２４ａの値から、移動体１０の現在の接線速度ｓ_ｒ’と、経路長ｓ_ｒと、旋回方向の角度（姿勢）φ_ｒと、旋回方向の角速度φ_ｒ’を算出する（Ｓ６）。すなわち、エンコーダ２２ａ，２４ａの値から車輪１２，１４の回転速度を算出し、この算出された回転速度と車輪１２，１４の幾何学的関係から移動体１０のｘ方向、ｙ方向、φ方向の速度を算出する（例えば、特許文献１に開示されたヤコビ行列を利用して算出することができる）。次いで、算出された各速度成分から、移動体１０の現在の接線速度ｓ_ｒ’と、経路長ｓ_ｒと、旋回方向の角度（姿勢）φ_ｒと、旋回方向の角速度φ_ｒ’を算出する。
ステップＳ８では、ジャイロセンサ２８で検出された傾斜角速度θ’を読込む。
ステップＳ１０では、ステップＳ２で読み込んだ時間ｔと、目標軌道入力部３８に記憶されている目標軌道データ（ｘ（ｔ），ｙ（ｔ））とから、目標接線速度ｓ’（ｔ）と目標経路長ｓ（ｔ）を算出する（進行方向目標値算出部４０の説明参照）。
次に、ステップＳ６で算出された現在の接線速度ｓ_ｒ’及び経路長ｓ_ｒと、ステップＳ８で読み込まれた傾斜角速度θ’と、ステップＳ１０で算出された目標接線速度ｓ’（ｔ）と目標経路長ｓ（ｔ）とを用いて、移動体１０の進行方向に関するモータ２２，２４のトルク指令値をそれぞれ算出する（Ｓ１２）。
次に、ステップＳ６で算出した現在の経路長ｓ_ｒを用いて、その経路長ｓ_ｒを実現すべきであった時刻ｔ’を求め（Ｓ１４）、その求めた修正時刻ｔ’から移動体１０の旋回方向の目標角度（目標姿勢）φと目標角速度φ’を算出する（ｓ１６）。（旋回方向目標値算出部５０の説明を参照）。
次に、ステップＳ６で算出された現在の旋回方向の角度（姿勢）φ_ｒ及び角速度φ_ｒ’と、ステップＳ１４で算出された旋回方向の目標角度（目標姿勢）φ及び目標角速度φ’とを用いて、移動体１０の旋回方向に関するモータ２２，２４のトルク指令値をそれぞれ算出する（Ｓ１８）。
ステップＳ２０では、ステップＳ１２で算出されたトルク指令値と、ステップＳ１８で算出されたトルク指令値を加算し、これらの値をモータ２２，２４（詳細には、モータ２２，２４を駆動するモータ駆動回路）に出力する。これによって、各車輪１２，１４が駆動されることとなる。ステップＳ２０が終わるとステップ２に戻り、次の制御タイミングにおける処理が開始される。
なお、ステップＳ２〜ステップＳ２０までの処理は、所定の時間間隔（例えば、１０ｍｓ）で行われる。これによって、移動体１０は、制御周期毎に、目標軌道からのずれが補正されることとなる。

上述した移動体１０が目標軌道（ｘ(ｔ），ｙ（ｔ）)からずれた場合における移動体１０の動作について、図７を参照して説明する。なお、移動体１０の制御周期はΔｔとする。図７では、ｘ−ｙ平面上に目標軌道が設定されており、この目標軌道に従って本実施例の移動体１０が走行した走行経路と、この目標軌道に従って従来技術に係る移動体が走行した走行経路とが模式的に示されている。図７に示すように、目標軌道として時刻ｔ_１のときの目標位置Ｐ_１（ｔ_１），時刻ｔ_１＋Δｔのときの目標位置Ｐ_２（ｔ_１＋Δｔ），時刻ｔ_１＋２Δｔのときの目標位置Ｐ_３（ｔ_１＋２Δｔ），時刻ｔ_１＋３Δｔのときの目標位置Ｐ_４（ｔ_１＋３Δｔ）が設定されている。移動体は時刻ｔ_１で目標位置Ｐ_１（ｔ_１）にいるものとし、この位置Ｐ_１（ｔ_１）から目標軌道（Ｐ_１→Ｐ_２→Ｐ_３→Ｐ_４→・）と移動するものとする。

まず、従来技術に係る倒立振子型の移動体の場合を説明する。移動体は、まず、時刻ｔ_１の位置Ｐ_１（ｔ_１）から時刻ｔ_１＋Δｔの目標位置Ｐ_２（ｔ_１＋Δｔ）に移動するための制御指令値を算出し、この制御指令値に基づいて車輪１２，１４を駆動する。この際、進行方向については速度応答性の遅れがあるため、移動体は目標位置Ｐ_１から目標位置Ｐ_２に到達するために必要な距離（経路長）を移動することができない。一方、旋回方向については速度応答性の遅れがなく、目標軌道で設定された通りに制御される。そのため、移動体は、時刻ｔ_１＋Δｔにおいて目標位置Ｐ_２からずれた位置Ｐ_２’に移動する。
次に、移動体は、目標位置Ｐ_２から目標位置Ｐ_３に移動するための制御指令値を算出し、この制御指令値に基づいて車輪１２，１４を駆動する。この際も、進行方向については速度応答性の遅れから、移動体は目標位置Ｐ_２から目標位置Ｐ_３に到達するために必要な距離（経路長）を移動することはできないものとする。一方、旋回方向については速度応答性の遅れがなく、目標軌道で設定された通りに制御される。そのため、移動体は、時刻ｔ_１＋２Δｔにおいて目標位置Ｐ_３からさらにずれた位置Ｐ_３’’に移動する。
以下、同様に目標位置Ｐ_３から目標位置Ｐ_４に移動するための制御指令値を算出し、この制御指令値に基づいて車輪１２，１４を駆動する。この場合も、進行方向の速度応答性の遅れから、移動体は目標軌道からさらにずれた位置Ｐ_４’’に移動する。
このように、従来技術の移動体では、進行方向の速度応答性の遅れから移動体が目標軌道からずれると、そのずれが修正されることなく制御指令値が算出される。このため、目標軌道からのずれが大きくなっていく。

次に、本実施例の移動体１０の場合を説明する。移動体１０は、上述の場合と同様に、まず、時刻ｔ_１の位置Ｐ_１（ｔ_１）から時刻ｔ_２の目標位置Ｐ_２（ｔ_１＋Δｔ）に移動するための制御指令値を算出し、この制御指令値に基づいて車輪１２，１４を駆動する。この場合も、進行方向については速度応答性の遅れから、移動体は目標位置Ｐ_１から目標位置Ｐ_２に到達するために必要な距離（経路長）を移動することはできない。一方、旋回方向については速度応答性の遅れがないため、目標軌道で設定された通りに制御される。そのため、移動体は、従来技術と同様、目標位置Ｐ_２からずれた位置Ｐ_２’に移動する。
次に、移動体１０は、進行方向については、目標位置Ｐ_２（ｔ_１＋Δｔ）から目標位置Ｐ_３（ｔ_１＋２Δｔ）に移動するための制御指令値を算出する。一方、旋回方向については、点Ｐ_１から点Ｐ_２’間の距離に対応した目標軌道上の点Ｐ_２ｎを基準として制御目標値を算出する。ここで、目標軌道どおりに移動体が移動した場合に、目標軌道上の点Ｐ_２ｎを移動体１０が時刻ｔ’で通過するものとする。旋回方向の制御目標値は、点Ｐ（ｔ’）（時刻ｔ’における目標軌道上の点）から点Ｐ（ｔ’＋Δｔ）（時刻ｔ’＋Δｔ）における目標軌道上の点）に移動するための制御指令値を算出する。次に、これらの制御目標値に基づいて車輪１２，１４が駆動される。このため、移動体は点Ｐ_２’から点Ｐ_３’に移動する。これによって、従来技術と比較して、目標軌道との誤差が小さくなる。
以下、同様に、進行方向については目標位置Ｐ_３（ｔ_１＋２Δｔ）から目標位置Ｐ_４（ｔ_１＋３Δｔ）に移動するための制御指令値を算出する。旋回方向については、移動体１０の現在の位置Ｐ_３’を基準として制御指令値が算出される。このため、移動体１０は点Ｐ_３’から点Ｐ_４’に移動することとなる。

上述した説明から明らかなように、本実施例の移動体１０では、進行方向の速度応答性の遅れから移動体１０が目標軌道からずれると、旋回方向については、そのずれた位置を基準として制御指令値が算出される。すなわち、進行方向の移動距離（実際に移動した経路長）に応じて、旋回方向の制御指令値が算出される。これによって、移動体１０の目標軌道からのずれが修正され、移動体１０は目標経路上を精度よく走行することができる。

（第２実施例） 次に、本発明の第２実施例に係る移動体について説明する。なお、第２実施例の移動体は、第１実施例の移動体と略同一の構成を備えており、制御装置６０の構成のみが相違する。以下では、第１実施例と異なる点を中心に説明する。

図８は第２実施例の制御装置６０の機能を示すブロック図である。図８に示すように、制御装置６０は、目標軌道入力部３８と、移動体の進行方向の制御指令値を算出するための進行方向目標値算出部６２と倒立制御コントローラ６４と目標接線速度算出部６６と、移動体の旋回方向の制御指令値を算出する旋回方向制御指令値算出部７０と、車輪角速度制御器６８を有している。

目標軌道入力部３８は、第１実施例と同様に、移動体の目標軌道（ｘ（ｔ），ｙ（ｔ））を記憶している。進行方向目標値算出部６２も、第１実施例と同様、移動体の目標軌道（ｘ（ｔ），ｙ（ｔ））を用いて、移動体１０の進行方向の制御目標値（目標接線速度ｓ’（ｔ）、目標経路長ｓ（ｔ））を算出する。算出された制御目標値（目標接線速度ｓ’（ｔ）、目標経路長ｓ（ｔ））は、倒立制御用コントローラ６４に入力される。

倒立制御用コントローラ６４も、第１実施例と同様に、エンコーダ２２ａ，２４ａで検出された車輪１２，１４の回転角速度から算出される移動体の経路長ｓ_ｒ（ｔ）と目標経路長ｓ（ｔ）との偏差を小さくし、エンコーダ２２ａ，２４ａで検出された車輪１２，１４の回転角速度から算出される移動体の接線速度ｓ_ｒ’（ｔ）と目標接線速度ｓ’（ｔ）との偏差を小さくし、かつ、ジャイロセンサ２８で検出された車体２０の傾斜角速度θ’に基づいて移動体１０が倒立を維持するように、各モータ２２，２４を駆動するための制御指令値を算出する。ただし、第２実施例の倒立制御用コントローラ６４は、第１実施例と異なり、制御指令値として、移動体の目標接線加速度（以下、新目標接線加速度という）を出力する。この倒立制御用コントローラ６４の設計手法としては、特許文献１に開示された設計手法と同様の手法を用いることができる。すなわち、τ（トルク）＝Ｉ（慣性モーメント）×ω’（角加速度）の関係から、特許文献１に示すような運動方程式を角加速度ω’を用いて表し、その運動方程式に対して特許文献１と同様の手法を適用して設計することができる。

目標接線速度算出部６６は、倒立制御用コントローラ６４で算出された新目標接線加速度を積分して新目標接線速度を算出する。この新目標接線速度は、旋回方向制御指令値算出部７０と車輪角速度制御器６８に出力される。

旋回方向制御指令値算出部７０は、目標軌道（ｘ（ｔ），ｙ（ｔ））と、目標接線速度算出部６６で算出された新目標接線速度（すなわち、進行方向の遅れが含まれたもの）を用いて、移動体１０の旋回方向の制御指令値（目標角速度φ’）を算出する。旋回方向制御指令値算出部７０は、複数の算出部７２，７４，７６，７８，８０，８２，８４によって構成され、これらが順に所定の演算を行うことで旋回方向の制御指令値（目標角速度φ’）を算出している。
具体的には、旋回方向制御指令値算出部７０は、まず、目標接線速度算出部６６で算出された新目標接線速度を用いて、移動体の推定経路長を算出する（新経路長算出部７６）。次に、移動体の推定される経路長から、その経路長を実現すべき時刻ｔ’を算出する（修正時刻算出部７４）。次に、算出した時刻ｔ’と目標軌道（ｘ（ｔ），ｙ（ｔ））から新しい目標位置（ｘ（ｔ’），ｙ（ｔ’））を算出する（新目標位置算出部７２）。新しい目標位置（ｘ（ｔ’），ｙ（ｔ’））が算出されると、その目標位置（ｘ（ｔ’），ｙ（ｔ’））から新目標速度を算出する（新目標速度算出部７８）。次いで、その算出された新目標速度から新目標加速度を算出し（新目標加速度算出部８０）、その新目標加速度と新目標速度から曲率１／Ｒを算出する（目標曲率算出部８２）。最後に、新目標速度算出部７８で算出された新目標速度と、目標曲率算出部８２で算出された曲率１／Ｒを掛けて、目標角速度φ’を算出する（目標角速度算出部８４）。この算出された目標角速度φ’は車輪角速度制御器６８に入力される。

車輪角速度制御器６８は、目標接線速度算出部６６で算出された新目標接線速度と、目標角度算出部８４で算出された目標角速度φ’を合成し、各モータ２２，２４に分配することで、各モータ２２，２４（各車輪１２，１４）の目標角速度を算出する。そして、算出された目標角速度とエンコーダ２２ａ，２４ａで検出される目標角速度との偏差が０となるようにモータ２２，２４を駆動する。

上述したことから明らかなように、第２実施例では、進行方向の制御指令値である新目標接線速度（進行方向の応答速度の遅れが含まれた接線速度）から今回の周期で移動する経路長を算出し、その経路長を実現すべき時刻ｔ’の目標位置（ｘ（ｔ’），ｙ（ｔ’））に基づいて旋回方向の制御指令値を算出する。すなわち、第２実施例では、進行方向の制御指令値から今回の周期で予測されるずれを考慮して、旋回方向の制御指令値が算出されている。

次に、上述のように構成される制御装置６０によって行われる処理について説明する。図９は制御装置６０の処理手順を示すフローチャートである。
図９に示すように、制御装置６０は、まず、制御装置６０が起動されてからの時間ｔ（制御開始時からの経過時間）を読み込む（Ｓ２２）。次に、各モータ２２，２４のエンコーダ２２ａ，２４ａの値（すなわち、車輪１２，１３の回転角速度）と、ジャイロセンサ２８の値（車体の傾斜角速度）を読込む（Ｓ２３）。また、読み込んだエンコーダ２２ａ，２４ａの値から、移動体１０の現在の接線速度ｓ_ｒ’と、経路長ｓ_ｒを算出する。
次に、ステップＳ２２で読み込んだ時刻ｔより移動体の目標接線速度及び目標経路長を算出し（Ｓ２４）、この算出した目標接線速度及び目標経路長と、エンコーダ２２ａ，２４ａの値から得られた現在の接線速度及び経路長と、ジャイロセンサ２８の傾斜角速度とから、倒立制御用の制御指令値（目標接線加速度）を算出する（Ｓ２６）。
ステップＳ２８では、ステップＳ２６で算出した制御指令値（加速度）を積分して新目標接線速度を算出する。次いで、ステップＳ２８で算出した新目標接線速度より新経路長を算出し（Ｓ３０）、この新経路長より修正時刻ｔ’を算出する（Ｓ３２）。次いで、ステップＳ３２で求めた修正時刻ｔ’から移動体１０の旋回方向の目標角速度φ’を算出する（Ｓ３４）。
次に、ステップＳ２８で算出した新目標接線速度と、ステップＳ３４で算出した目標角速度φ’を合成し、その合成したものを各モータ１２，１４に分配し、各モータ１２，１４の目標角速度を算出する（Ｓ３６）。
次いで、ステップＳ３６で算出された目標角速度とエンコーダ２２ａ，２４ａで測定された車輪回転速度の偏差が０となるように、モータ２２，２４を制御する（Ｓ３８）。これによって、各車輪１２，１４が駆動されることとなる。ステップＳ３８が終わるとステップ２２に戻り、次の制御タイミングにおける処理が開始される。

上述した説明から明らかなように、第２実施例の移動体では、進行方向の制御指令値である新目標接線速度を用いて移動体の目標位置（進行方向の遅れを含んだ目標位置）を算出し、この目標位置に基づいて旋回方向の制御指令値（目標角速度）を算出する。このため、進行方向の速度応答の遅れによる目標軌道からのずれを見込んでモータ２２，２４が駆動されることとなり、移動体の目標軌道からのずれが未然に防止される。
また、第２実施例では、エンコーダ２２ａ，２４ａの検出結果が車輪角速度制御器６８にフィードバックされる構成とされている。このため、フィードバックループが小さくなり、モータ２２，２４を所望の角度に精度良く制御できる。これによって、移動体は、目標軌道によって規定される目標経路上を精度良く走行することができる。

（第３実施例） 次に、本発明の第３実施例に係る移動体について説明する。なお、第３実施例の移動体は、移動体の位置（ｘ、ｙ）と姿勢（φ）を検出するセンサを備え、そのセンサの検出結果を用いる点で、第１実施例や第２実施例の移動体とは異なる。以下の説明では、上述した実施例とは異なる点を中心に説明する。

図１０は第３実施例の移動体の制御装置８８の機能を示すブロック図である。図１０に示すように、制御装置８８には位置・姿勢センサ８６とジャイロセンサ２８とエンコーダ２２ａ，２４ａが接続されている。
位置・姿勢センサ８６は、移動体の位置（ｘ，ｙ）と姿勢（φ）を検出する。位置・姿勢センサ８６には、例えば、レーザレンジセンサを用いることができる。レーザレンジセンサは、レーザを照射することによって、レーザが照射された物体までの距離を検出する。このため、移動体が走行する部屋の壁の形状を記憶したマップと、レーザを全方位（３６０度）に照射したときの壁の形状とを照合することで、移動体の位置と姿勢（ｘ，ｙ，φ）を得ることができる。位置・姿勢センサ８６の検出結果（ｘ，ｙ，φ）は、制御装置８８に入力される。なお、位置・姿勢センサ８６には、ＧＰＳセンサやモーションキャプチャ等を用いることもできる。

制御装置８８は、位置・姿勢センサ８６の検出結果（ｘ，ｙ，φ）と、ジャイロセンサ２８の検出結果θ’と、エンコーダ２２ａ，２４ａの検出結果とに基づいて、モータ２２，２４を制御する。制御装置８８は、様々な機能３８，９０〜１１２を備えており、これらが所定の演算を行うことでモータ２２，２４を制御するための制御指令値（目標角速度）を算出している。
具体的には、制御装置８８は、目標軌道入力部３８に記憶されている目標軌道（ｘ（ｔ），ｙ（ｔ））を微分することで、移動体の目標速度（ｘ’（ｔ），ｙ’（ｔ））を算出する。そして、φ＝Ａｒｃｔａｎ（ｙ’（ｔ）／ｘ’（ｔ））の式にｘ’（ｔ）とｙ’（ｔ）を代入し、移動体の目標姿勢φを算出する（目標姿勢算出部９０）。
目標姿勢が算出されると、その目標姿勢と位置・姿勢センサで検出された移動体の姿勢（φ）から姿勢偏差Δφを算出し（偏差算出部９２）、その姿勢偏差Δφを軌道トラッキングコントローラ１００に入力する。
また、目標軌道（ｘ（ｔ），ｙ（ｔ））から算出される移動体の位置と、位置・姿勢センサ８６の検出結果から算出される現在の移動体の位置とを用いて、移動体の進行方向の位置偏差を算出し（偏差算出部９４）、その位置偏差を軌道トラッキングコントローラ１００に入力する。
また、目標軌道（ｘ（ｔ），ｙ（ｔ））から算出される移動体の位置と、位置・姿勢センサ８６の検出結果から算出される現在の移動体の位置とを用いて、移動体の進行方向に直交する方向の位置偏差を算出し（偏差算出部９６）、その偏差を軌道トラッキングコントローラ１００に入力する。

軌道トラッキングコントローラ１００は、上記の各偏差を入力とし、これらの偏差を０とするような目標接線速度と目標角速度を算出する。軌道トラッキングコントローラ１００の具体的な構成例としては、例えば、特開平３−２４６０５号公報に開示されている。

制御装置８８は、軌道トラッキングコントローラ１００によって算出された目標接線速度を倒立制御用コントローラ１１０に入力する。また、算出された目標接線速度を積分して目標経路長を算出し（経路長算出部１０２）、その目標経路長を倒立制御用コントローラ１１０に入力する。倒立制御用コントローラ１１０は、ジャイロセンサ２８の検出結果、エンコーダ２２ａ，２４ａの検出結果、並びに入力された目標接線速度及び目標経路長に基づいて、移動体の進行方向の制御指令値（新接線加速度）を算出する。なお、倒立制御用コントローラ１１０は、第２実施例の倒立制御用コントローラ６４と同一である。
次に、倒立制御用コントローラ１１０で算出された新接線加速度を積分して新接線速度を算出する（新接線速度算出部１０８）。算出された新接線速度と軌道トラッキングコントローラ１００によって算出された目標接線速度の比（新接線速度／目標接線速度）を算出する（速度比算出部１０４）。すなわち、倒立制御用コントローラ１１０で処理された新接線速度は、移動体の進行方向の応答遅れが考慮されたものとなっている。一方、軌道トラッキングコントローラ１００で算出された目標接線速度は、移動体の進行方向の応答遅れが考慮されていない。このため、新接線速度と目標接線速度の比を求めることで、移動体の進行方向の速度応答の遅れの程度が算出される。
次に、軌道トラッキングコントローラ１００で算出された目標角速度に上記の比（新接線速度／目標接線速度）を掛けて、移動体の旋回方向の制御指令値（目標角速度）を算出する（旋回方向の新目標算出部１０６）。
車輪角速度制御器１１２は、新接線速度算出部１０８で算出された制御指令値（新接線速度）と、目標角度算出部１０６で算出された制御指令値（目標角速度φ’）を合成して、各モータ２２，２４に分配することで、各モータ２２，２４（各車輪１２，１４）の目標角速度を算出する。そして、算出された目標角速度とエンコーダ２２ａ，２４ａで検出される目標角速度との偏差が０となるようにモータ２２，２４を駆動する。

次に、上述のように構成される制御装置８８によって行われる処理について説明する。図１１は制御装置８８の処理手順を示すフローチャートである。
図１１に示すように、制御装置８８は、まず、制御装置８８が起動されてからの時間ｔ（すなわち、制御開始時からの経過時間）を読み込む（Ｓ４２）。次に、位置・姿勢センサ８６の出力を読み込む（Ｓ４３）。また、ジャイロセンサ２８の出力、エンコーダ２２ａ，２４ａの出力を読み込む（Ｓ４４）。
次に、目標軌道から目標接線速度を算出し、この目標接線速度から目標姿勢（φ）を算出する（Ｓ４６）。そして、位置・姿勢センサ８６の出力と目標軌道から、各偏差（Δｘ，Δｙ，Δφ）を算出する（Ｓ４８）。次いで、ステップＳ４８で算出した偏差を用いて、軌道トラッキングコントローラ１１０が移動体の目標接線速度と目標角速度（旋回方向の目標角速度）を算出する（Ｓ５０）。ステップＳ５０で目標接線速度が算出されると、その目標接線速度を積分して目標経路長が算出される（Ｓ５２）。次に、ステップＳ４４で検出された検出結果と、ステップＳ５０で算出された目標接線速度と、ステップＳ５２で算出された目標経路長とを用いて、倒立制御用の新接線加速度を算出する（Ｓ５４）。
ステップＳ５２で算出された新接線加速度は積分されて新接線速度が算出される（Ｓ５６）。次に、ステップＳ５０で算出した目標接線速度とステップＳ５６で算出した新接線速度の速度比（新接線速度／目標接線速度）を算出し（Ｓ５８）、この速度比（新目標接線速度／目標接線速度）をステップＳ５０で算出された目標角速度に掛け、旋回方向の目標角速度を修正する（Ｓ６０）。
次に、ステップＳ５６で算出した新接線速度と、ステップＳ６０で算出した目標角速度φ’を合成し、その合成したものを各モータ１２，１４に分配し、各モータ１２，１４の目標角速度を算出する（Ｓ６２）。
次いで、ステップＳ６２で算出された目標加速度とエンコーダ２２ａ，２４ａで測定された車輪回転速度の偏差が０となるように、モータ２２，２４を制御する（Ｓ６４）。これによって、各車輪１２，１４が駆動されることとなる。ステップＳ６４が終わるとステップ４２に戻り、次の制御タイミングにおける処理が開始される。

上述した説明から明らかなように、第３実施例の移動体では、位置・姿勢センサ８６の検出結果を用いて移動体が目標経路上を移動するよう制御する。また、旋回方向の目標角速度は、進行方向の速度応答性の遅れを考慮して修正されている。これらによって、移動体は目標経路上を精度良く走行することができる。

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

１０：移動体
１２、１４：車輪
１６：車軸
２０：車体
２２、２４：モータ
２２ａ、２４ａ：エンコーダ
２８：ジャイロセンサ
３０，６０，８８：制御装置

Claims (2)

1. 車輪と、車輪に支持される車体を備えており、車輪を回転駆動することで車体を倒立状態に維持しながら路面上を走行する倒立振子型の移動体であって、
   移動体の進行方向の位置に関する情報を測定するためのセンサと、
   移動体の目標位置を時間の関数で表した目標軌道を用いて、車体が倒立を維持しつつ移動体の進行方向の位置を制御するための第１制御指令値を算出する手段と、
   前記目標軌道とセンサで実測される移動体の進行方向の位置に関する情報とを用いて、移動体の旋回方向の角度を制御するための第２制御指令値を算出する手段と、
   第１制御指令値と第２制御指令値に基づいて車輪を駆動する手段と、を備えており、
   第１制御指令値算出手段と第２制御指令値算出手段と駆動手段とを所定周期毎に繰り返し実行することで、移動体が路面上を走行し、
   第２制御指令値算出手段は、センサの検出結果から移動体の経路長さを算出し、その算出された経路長さに対応する前記目標軌道上の位置を算出し、その算出された位置を用いて移動体の旋回方向の角度を制御するための第２制御指令値を算出することを特徴とする倒立振子型移動体。
2. 車輪と、車輪に支持される車体と、進行方向の位置に関する情報を測定するためのセンサと、車輪を回転駆動する駆動装置と、駆動装置を制御する制御装置を備えており、駆動装置によって車輪を回転駆動することで車体を倒立状態に維持しながら路面上を走行する倒立振子型移動体の制御方法であって、
   移動体の目標位置を時間の関数で表した目標軌道を用いて、車体が倒立を維持しつつ移動体の進行方向の位置を制御するための第１制御指令値を算出する工程と、
   前記目標軌道とセンサで実測される移動体の進行方向の位置に関する情報とを用いて、移動体の旋回方向の角度を制御するための第２制御指令値を算出する工程と、
   第１制御指令値と第２制御指令値に基づいて車輪を駆動する工程と、を有しており、
   上記各工程を所定周期毎に繰返し実行することで移動体を制御し、
   前記第２制御指令値を算出する工程は、センサの検出結果から移動体の経路長さを算出し、その算出された経路長さに対応する前記目標軌道上の位置を算出し、その算出された位置を用いて移動体の旋回方向の角度を制御するための第２制御指令値を算出することを特徴とする倒立振子型移動体の制御方法。

Images