JP2017045250A - 全方位移動体、その制御方法及びプログラム - Google Patents

Abstract

【課題】アクチュエータの出力性能を考慮して、最適な制御を行うこと。
【解決手段】全方位移動体の台車部及び本体部の重心及び鉛直方向軸周りに関する運動方程式に基づいて、慣性項と遠心力及びコリオリ項と粘性摩擦項と重力項とを含む駆動輪及び旋回軸のトルクに関する運動方程式が算出され、該運動方程式と指令軌道とに基づいて駆動輪及び旋回軸のトルクを制約するトルク制約条件が算出される。予め設定された駆動輪及び旋回軸の速度の制約に基づいて駆動輪及び旋回軸の速度を制約する速度制約条件が算出される。算出されたトルク制約条件および速度制約条件を満たし、かつ、指令軌道を追従する軌道であって軌道パラメータ及びその微分値の平面上の軌道が算出される。全方位移動体は、算出された軌道に基づいて本体部の位置姿勢の目標値を算出し、該算出した本体部の位置姿勢の目標値に基づいて駆動制御する。
【選択図】図３

Description

本発明は、複数の駆動輪が設けられた台車部と、台車部に旋回軸を介して回転可能に設けられた本体部と、を備え、全方位に移動可能な全方位移動体、その制御方法及びプログラムに関するものである。

局所地図データにおける経路条件（距離や路面状況）によりそれぞれ規定される各許容速度の値の中から最低速度の値を速度指令として速度計画を行い、駆動操舵系を制御する移動体が知られている（特許文献１参照）。

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

ところで、全方位移動体において、台車部の駆動輪及び本体部の旋回軸を駆動するアクチュエータの出力性能には限界が存在する。このため、駆動輪及び旋回軸の駆動トルク及び回転速度に限界が生じる。また、全方位移動体を大きく加速させて移動させようとすると、駆動輪と路面との間にはスリップが発生する。しかしながら、上記移動体は、そのようなアクチュエータの出力性能を考慮して制御を行っていない。

本発明は、このような問題点に鑑みてなされたものであり、アクチュエータの出力性能を考慮して、最適な制御を行うことができる全方位移動体、その制御方法及びプログラムを提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、複数の駆動輪が設けられた台車部と、台車部に旋回軸を介して回転可能に設けられた本体部と、前記駆動輪を駆動する第１駆動手段と、前記旋回軸を駆動する第２駆動手段と、を備え、全方位に移動可能な全方位移動体であって、前記台車部及び本体部の重心及び鉛直方向軸周りに関する運動方程式に基づいて、慣性項と、遠心力及びコリオリ項と、粘性摩擦項と、重力項と、を含む前記駆動輪及び旋回軸のトルクに関する運動方程式が算出され、該運動方程式と、予め設定された当該全方位移動体が移動すべき軌道であって軌道パラメータで表現された指令軌道と、に基づいて、前記駆動輪及び旋回軸のトルクを制約する条件であって、前記軌道パラメータで表現されたトルク制約条件が算出され、予め設定された前記駆動輪及び旋回軸の速度の制約に基づいて、前記駆動輪及び旋回軸の速度を制約する条件であって、前記軌道パラメータで表現された速度制約条件が算出され、前記算出されたトルク制約条件および速度制約条件を満たし、かつ、前記指令軌道を追従する軌道であって、前記軌道パラメータ及びその微分値の平面上に表現された前記軌道が算出され、前記算出された軌道に基づいて前記本体部の位置姿勢の目標値を算出し、該算出した本体部の位置姿勢の目標値に基づいて前記第１及び第２駆動手段を制御する制御手段を備える、ことを特徴する全方位移動体である。
この一態様において、前記算出された軌道は、前記算出されたトルク制約条件および速度制約条件を満たし、かつ、前記指令軌道を最短時間で追従する最短時間軌道であってもよい。
この一態様において、前記算出された軌道は、前記算出されたトルク制約条件および速度制約条件に基づいて、軌道パラメータ及びその微分値の平面における始点および終点から所定の積分幅で前進積分及び後退積分を繰り返して最小加速度から最大加速度へと切り替わるスイッチングポイントを探索することで算出された前記最短時間軌道であってもよい。
上記目的を達成するための本発明の一態様は、複数の駆動輪が設けられた台車部と、台車部に旋回軸を介して回転可能に設けられた本体部と、前記駆動輪を駆動する第１駆動手段と、前記旋回軸を駆動する第２駆動手段と、を備え、全方位に移動可能な全方位移動体の制御方法であって、前記台車部及び本体部の重心及び鉛直方向軸周りに関する運動方程式に基づいて、慣性項と、遠心力及びコリオリ項と、粘性摩擦項と、重力項と、を含む前記駆動輪及び旋回軸のトルクに関する運動方程式を算出し、該運動方程式と、予め設定された当該全方位移動体が移動すべき軌道であって軌道パラメータで表現された指令軌道と、に基づいて、前記駆動輪及び旋回軸のトルクを制約する条件であって、前記軌道パラメータで表現されたトルク制約条件を算出するステップと、予め設定された前記駆動輪及び旋回軸の速度の制約に基づいて、前記駆動輪及び旋回軸の速度を制約する条件であって、前記軌道パラメータで表現された速度制約条件を算出するステップと、前記算出されたトルク制約条件および速度制約条件を満たし、かつ、前記指令軌道を追従する軌道であって、前記軌道パラメータ及びその微分値の平面上に表現された前記軌道を算出するステップと、
前記算出した軌道に基づいて前記本体部の位置姿勢の目標値を算出し、該算出した本体部の位置姿勢の目標値に基づいて前記第１及び第２駆動手段を制御するステップと、を含む、ことを特徴する全方位移動体の制御方法であってもよい。
上記目的を達成するための本発明の一態様は、複数の駆動輪が設けられた台車部と、台車部に旋回軸を介して回転可能に設けられた本体部と、前記駆動輪を駆動する第１駆動手段と、前記旋回軸を駆動する第２駆動手段と、を備え、全方位に移動可能な全方位移動体のプログラムであって、前記台車部及び本体部の重心及び鉛直方向軸周りに関する運動方程式に基づいて、慣性項と、遠心力及びコリオリ項と、粘性摩擦項と、重力項と、を含む前記駆動輪及び旋回軸のトルクに関する運動方程式を算出し、該運動方程式と、予め設定された当該全方位移動体が移動すべき軌道であって軌道パラメータで表現された指令軌道と、に基づいて、前記駆動輪及び旋回軸のトルクを制約する条件であって、前記軌道パラメータで表現されたトルク制約条件を算出する処理と、予め設定された前記駆動輪及び旋回軸の速度の制約に基づいて、前記駆動輪及び旋回軸の速度を制約する条件であって、前記軌道パラメータで表現された速度制約条件を算出し、前記算出されたトルク制約条件および速度制約条件を満たし、かつ、前記指令軌道を追従する軌道であって、前記軌道パラメータ及びその微分値の平面上に表現された前記軌道を算出する処理と、前記算出した軌道に基づいて前記本体部の位置姿勢の目標値を算出し、該算出した本体部の位置姿勢の目標値に基づいて前記第１及び第２駆動手段を制御する処理と、をコンピュータに実行させる、ことを特徴するプログラムであってもよい。

本発明によれば、アクチュエータの出力性能を考慮して、最適な制御を行うことができる全方位移動体、その制御方法及びプログラムを提供することができる。

（ａ）本発明の一実施形態に係る全方位移動体の概略的構成を示す上面図である。（ｂ）本発明の一実施形態に係る全方位移動体の概略的構成を示す正面図である。（ｃ）本発明の一実施形態に係る全方位移動体の概略的構成を示す側面図である。 本発明の一実施形態に係る全方位移動体の概略的なシステム構成を示すブロック図である。 本発明の一実施形態に係る制御装置の概略的なシステム構成を示すブロック図である。 軌道パラメータｓ−ｓ（ドット）平面上におけるＭＶＣ、ＶＬＣ及び最短時間軌道を示す図である。 本発明の一実施形態に係る全方位移動体の座標系の定義を示す図である。 本発明の一実施形態に係る全方位移動体の制御方法のフローを示すフローチャートである。 指令軌道を示す図である。 左駆動輪の駆動トルクを示す図である。 右駆動輪の駆動トルクを示す図である。 旋回軸の駆動トルクを示す図である。 左右駆動輪および旋回軸の駆動トルクが制約されたときの、軌道パラメータｓ−ｓ（ドット）平面上の軌道を示す図である。 ＭＶＣ及びＶＬＣ曲線の下を通過しながら、滑らかで実現可能な軌道の一例を示す図である。

以下、図面を参照して本発明の実施形態について説明する。
図１（ａ）乃至（ｂ）は、本発明の一実施形態に係る全方位移動体の概略的構成を示す図である。本発明の実施形態に係る全方位移動体１は、例えば、アクティブキャスタ式の全方位移動ロボットとして構成されている。

全方位移動体１は、移動を行う台車部２と、台車部２の上方側に旋回動可能に連結された本体部３と、を有している。台車部２は、一つの補助輪２１と左右一対の駆動輪２２が設けられている。本体部３は、台車部２に対して、旋回軸（Ｙａｗ回転軸）４を中心にして相対回動する。駆動輪２２の駆動軸と本体部３の旋回軸４とはオフセットしている（交差しない）。

全方位移動体１は、駆動軸と旋回軸４とを独立して駆動することで、全方位移動を実現する。全方位移動体１は、例えば、駆動軸による駆動輪２２の回転差に加えて、旋回軸４による本体部３の相対回動によって全方位移動を実現する。また、本体部３には、例えば、物体を把持できる多関節型アームが回動可能に設けられていてもよい。これにより、全方位移動体１を任意の方向に移動させつつ、多関節型アームを動作させることで、物体を容易に把持し移動させることができる。

図２は、本実施形態に係る全方位移動体の概略的なシステム構成を示すブロック図である。本実施形態に係る全方位移動体１は、左右の駆動輪２２の回転情報(回転角、回転速度、回転角加速度)を検出する一対の回転センサ５、旋回軸４の回転情報を検出する回転センサ６、一対の駆動輪２２を駆動する一対の車輪アクチュエータ７と、旋回軸４を駆動する旋回アクチュエータ８と、車輪アクチュエータ７および旋回アクチュエータ８を制御する制御装置９と、を備えている。
制御装置９は、回転センサ５、６により検出される回転情報に基づいて、車輪アクチュエータ７及び旋回アクチュエータ８を制御する。

制御装置９は、例えば、演算処理等を行うＣＰＵ（Central Processing Unit）９ａ、ＣＰＵ９ａによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ９ｂ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）９ｃなどからなるマイクロコンピュータを中心にして、ハードウェア構成されている。ＣＰＵ９ａ、メモリ９ｂ及びインターフェイス部９ｃは、データバスなどを介して相互に接続されている。

各回転センサ５は、例えば、左右の駆動輪２２の駆動軸に夫々設けられている。各回転センサ５は、ポテンショメータ、エンコーダなどで構成されている。各車輪アクチュエータ７は、第１駆動手段の一具体例である。各車輪アクチュエータ７は、例えば、左右の駆動輪２２の駆動軸に連結されている。各車輪アクチュエータ７は、モータなどで構成されている。

旋回アクチュエータ８は、第２駆動手段の一具体例である。旋回アクチュエータ８は、例えば、旋回軸４に連結されている。各旋回アクチュエータ８は、モータなどで構成されている。車輪アクチュエータ７及び旋回アクチュエータ８は、制御装置９から出力される制御信号に応じて回転駆動する。

ところで、全方位移動体における駆動輪及び旋回軸を駆動するアクチュエータの出力性能には限界が存在する。このため、駆動輪及び旋回軸の駆動トルク及び回転速度に限界が生じる。また、台車部を大きく加速させて移動させようとすると、駆動輪と路面との間にはスリップが発生する。
特に、全方位移動体の制御において、上記アクチュエータの出力性能は、その軸のトルクだけで制約されるわけではなく、駆動輪が滑らない静摩擦や全方位移動体の重心の動的効果を合わせたＺＭＰ（Zero Moment Point）のような指標で律速させる点で特異である。

これに対し、本実施形態における全方位移動体１は、上述した駆動輪２２及び旋回軸４の駆動トルク及び回転速度の制約条件を満たしつつ、設定された指令軌道に最短時間で追従する最短時間制御（Time Optimal Control）を実行する。
これにより、上記静摩擦やＺＭＰのような指標で律速させる特異な点を含めてアクチュエータの出力性能を考慮しつつ、最短時間での制御を行うことができる。

なお、最短時間制御は周知の制御方法であるため、詳細な説明は省略する。最短時間制御の詳細は、例えば非特許文献：J.Bobrow、“Optimal robot path planning using the minimum-time criterion”Robotics and Automation, IEEE Journal vol.4,pp.443(1988)などに開示されており、本実施形態において適宜これを援用できる。
一般に、全方位移動体は、低い自由度で全方位移動可能に構成でき、過拘束とならないことや耐荷重が高く取れるなどのメリットを有しているが、一方で、駆動輪及び旋回軸のトルク及び速度の制約条件を満たした軌道を求めることが困難であった。本実施形態においては、上述の如く、全方位移動体１の制御に最短時間制御を適用することで、駆動輪２２及び旋回軸４の駆動トルク及び回転速度の制約条件を満たしつつ、設定された指令軌道に最短時間で追従する最短時間軌道を容易に生成できる。

図３は、本実施形態に係る制御装置の概略的なシステム構成を示すブロック図である。本実施形態に係る制御装置９は、トルク制約条件および速度制約条件を算出する制約条件算出部９１と、最短時間軌道を算出する軌道算出部９２と、最短時間軌道に基づいて車輪アクチュエータ７および旋回アクチュエータ８を制御すると制御部９３と、を有している。

制約条件算出部９１は、台車部２及び本体部３の重心及び鉛直方向軸周りに関する運動方程式に基づいて、慣性項と、遠心力及びコリオリ項と、粘性摩擦項と、重力項と、を含む駆動輪２２及び旋回軸４のトルクに関する運動方程式を算出する。さらに、制約条件算出部９１は、この運動方程式と、予め設定された全方位移動体１が移動すべき軌道であって軌道パラメータで表現された指令軌道と、に基づいて、駆動輪２２及び旋回軸４のトルクを制約する条件であって、軌道パラメータで表現されたトルク制約条件を示す式を算出する。

制約条件算出部９１は、予め設定された駆動輪２２及び旋回軸４の速度の制約に基づいて、駆動輪２２及び旋回軸４の速度を制約する条件であって、軌道パラメータで表現された速度制約条件を示す式を算出する。
なお、制約条件算出部９１は、オフラインで、上記トルク制約条件及び速度制約条件を算出しているが、これに限定されず、オンラインで上記トルク制約条件及び速度制約条件を算出してもよい。

軌道算出部９２は、制約条件算出部９１により算出されたトルク制約条件および速度制約条件を満たし、かつ、指令軌道を最短時間で追従する軌道であって、軌道パラメータを横軸とし、その微分値を縦軸とした平面（以下、軌道パラメータｓ−ｓ（ドット）平面と称す）上に表現された最短時間軌道を算出する。なお、軌道算出部９２は、オフラインで、上記最短時間軌道を算出しているが、これに限定されず、オンラインで上記最短時間軌道を算出してもよい。

制御部９３は、制御手段の一具体例である。制御部９３は、軌道算出部９２により算出された最短時間軌道に基づいて、全方位移動体１の位置姿勢の目標値を算出する。制御部９３は、該算出した全方位移動体１の位置姿勢の目標値に基づいて、車輪アクチュエータ７および旋回アクチュエータ８を制御することで、台車部２の駆動輪２２および本体部３の旋回軸４を制御する。

次に、全方位移動体の駆動輪及び旋回軸のトルクの制約条件について詳細に説明する。上述の如く、駆動輪２２及び旋回軸４を駆動する車輪及び旋回アクチュエータ７、８の出力性能を考慮して、駆動輪２２及び旋回軸４のトルクに制約条件を付加する必要がある。

ここで、全方位移動体１を、複数の関節を有するロボットとして考えることができる。すなわち、全方位移動体１の駆動輪２２及び旋回軸４による本体部３の自由度は、仮想的なロボットの関節ととらえることができる。ロボットの関節において、その速度、加速度、トルク等に制約が存在する。この制約は、例えば、定数の上下限値や関数などの曲線で与えられる。ここで、ロボットが動作すべき指令軌道が空間軌道として与えられる場合を考える。この場合、空間軌道は、一つの軌道パラメータｓ（時間をパラメータ化したもの）で表現できるものとし、軌道パラメータｓに対して空間軌道は区間的に２階微分まで連続であるとする。

ロボットの関節角は、その空間軌道上の１点が指定されれば、決定される。また、ロボットのある状態の速度や加速度も、空間軌道上の対応点における、軌道パラメータｓの速度ｓ（ドット）、加速度ｓ（２ドット）で表現できる。

以上の仮定に基づけば、ロボットの関節の速度、加速度、及びトルクに対する制約条件は、軌道パラメータｓ、ｓ（ドット）、ｓ（２ドット）に関する不等式として表現できる。なお、ｓの時間に対する１階微分値をｓ（ドット）、ｓの時間に対する２階微分値をｓ（２ドット）と表記し、以降の他のパラメータについても同様に表記する。

仮に、この軌道パラメータの不等式が下記（１）式のような式である場合、関節に設定された制約条件を全て満たしながら、与えられた空間軌道を最短時間で追従するための速度軌道は、後述するアルゴリズムに従って求めることができる。

但し、上記（１）式において、ｎは制約条件の数であり、上限及び下限の制約を分けて、それぞれ１つと数える。以下、本実施形態において、上記（１）式を最短時間制御の制約条件の標準形と称す。

最短時間制御では、軌道パラメータｓ−ｓ（ドット）平面上で軌道を探索する。図４は、軌道パラメータｓ−ｓ（ドット）平面の一例を示す図である。図４において、ロボットの最短時間軌道は、始点（ｓ＝０）から終点（ｓ＝１）までの曲線となる。

最短時間制御の制約条件の標準形は、軌道パラメータｓ−ｓ（ドット）平面上の曲線で区分された領域として表現される。これら曲線は、到達不可能な領域（inadmissible region）を示すＭＶＣ（Maximum Velocity Curve）と、速度の上限を示すＶＬＣ（Velocity Limit Curve）とを含む。ロボットの関節の速度、加速度、トルクの制約条件は、これらＭＶＣ及びＶＬＣで表現できる。

与えられた空間軌道を最短時間で追従するための最短時間軌道は、ＭＶＣ及びＶＬＣ曲線の下を通過しながら、例えば、軌道上の各時刻では加速度の最大値もしくは最小値で加減速するものとなる。より詳細には、最短時間軌道は、ＭＶＣ及びＶＬＣ曲線の下を通過しながら、空間軌道を追従できるような駆動輪２２及び旋回軸４の加速度のうちのいずれかが、最大加速度もしくは最小加速度になっているような軌道である。このような最短時間軌道は、例えば、図４において、Time optimal trajectoryとして示されている。

次に、本実施形態に係る全方位移動体１について、全方位移動体１の機構運動学および同力学モデルから、上記（１）式に示す最短時間制御の制約条件の標準形を導出する。
図５は、本実施形態に係る全方位移動体の座標系の定義を示す図である。ここで、世界座標系をΣ_０とする。駆動輪２２の車軸中心に台車座標系Σ_１を設定し、台車部２の正面方向をｘ軸とする。本体部３の回転軸中心（旋回軸４）に本体座標系Σ_２を設定し、本体部３の正面方向をｘ軸とする。なお、世界座標系Σ_０、台車座標系Σ_１、本体座標系Σ_２において、鉛直方向の軸（鉛直方向軸）をｚ軸とする。以降の数式において、座標系Σ_ｎからΣ_ｍへの相対的な変換行列を^ｎＴ_ｍのように、左上に基準となる座標系の添字、右下に対象座標系の添字を記述する。座標系Σ_ｎからΣ_ｍへ相対位置を^ｎｐ_ｍと記述し、相対角度を^ｎθ_ｍと記述し、その他のパラメータ（速度、力、トルクなど）についても同様に記述する。省略された場合は０（世界座標系基準）とする。ベクトルをｖ_１のように太字で、その（ｘ、ｙ、ｚ）の要素をそれぞれ（ｖ_１、ｘ、ｖ_１、ｙ、ｖ_１、ｚ）のように記述する。

駆動輪２２の速度と台車部２の速度及び角速度との関係は、下記（２）及び（３）式で与えられる。

上記式において、ｖ_１は台車部２の並進速度、ω_１は台車部２の角速度、ω_ｌ、ω_ｒは左右駆動輪２２の速度、ｒは駆動輪２２の半径、ｄは台車部２のトレッドである。

世界座標系から台車座標系への姿勢行列は（^０Ｒ_１）であり、位置ベクトルは（^０ｐ_１）であり、台車座標系から本体座標系への姿勢行列は（^１Ｒ_２）であり、相対位置ベクトルは（^１ｐ_２）である。姿勢行列（^０Ｒ_１）及び（^１Ｒ_２）は、下記式で表現される。

但し、^０θ_１は台車部２の姿勢角度であり、^１θ_２は旋回軸４の角度である。本体部３の姿勢角度^０θ_２は、下記式で表現される。
^０θ_２＝^０θ_１＋^１θ_２ （６）

次に、台車部２及び本体部３の運動方程式をニュートン・オイラー方法で定式化し、駆動輪２２及び旋回軸４のトルクの関係式を導出する。
本体部３の重心の運動方程式は、下記式で表現できる。
ｆ_２＝ｍ_２ ^０ｐ（２ドット）_２,cog （７）

上記式において、ｆ_２は本体部３に対する外力、p_２,cogは本体部３の重心位置、ｍ_２は本体部３の質量である。
本体部３のｚ軸周りの運動方程式は、下記式で表現できる。

上記式において、^０τ_２は本体部３のｚ軸周りのトルク、Ｉ_２、ｚは本体部３の重心ｚ軸周りの慣性モーメントである。また、記号｜_ｚは、ベクトルのｚ要素である。
台車部２の重心の運動方程式は、下記式で表現できる。

上記式において、ｆ_１は台車部２が受ける力、ｍ_１は台車部２の質量である。台車部２のｚ軸周りの運動方程式は、下記式で表現できる。

上記式において、^０τ_１は台車部２のｚ軸周りのトルク、Ｉ_１、ｚは台車部２の重心ｚ軸周りの慣性モーメントである。
台車部２への外力およびトルクと左右駆動輪２２のトルクとの関係は下記式で表現できる。

上記（１）式乃至（１２）式を連立して解くと、台車部２および本体部３の運動のための、左右駆動輪２２および旋回軸４の駆動トルクτは、下記式のように導出される。

上記式において、τ＝（τ_ｌ、τ_ｒ、^１τ_２）、ｑ＝（ｘ_２、ｙ_２、θ_２）である。Ｍ（ｑ）、Ｃ（ｑ、ｑ（ドット））、Ｂ（ｑ）、ｇ（ｑ）は、夫々、慣性行列、遠心力とコリオリの項、粘性摩擦行列、重力項、である。

このように、台車部２及び本体部３の重心及びｚ軸周りに関する運動方程式（７）式乃至（１０）式に基づいて、慣性項と、遠心力及びコリオリ項と、粘性摩擦項と、重力項と、を含む駆動輪２２及び旋回軸４のトルクに関する運動方程式（１３）式が算出される。

全方位移動体１の本体部３の位置姿勢ｑ＝（ｘ_２、ｙ_２、ｚ_２）の空間的な指令軌道は、予め設定されているものとする。この指令軌道は、例えば、複数の経由点で表現されたスプライン補間による軌道として与えられる。この指令軌道ｆ（ｓ）は、軌道パラメータｓによってパラメータ化されているものとする。ここで、ｓは線長としてもよいが、簡略化のため、始点ｓ＝０、経由点ｎで終点ｓ＝ｎとなるようなパラメータとする。

上記運動方程式（１３）式は、下記式を用いて、軌道パラメータｓにより１次元化することができる。

上記（１３）式に、上記（１４）乃至（１６）式を代入することで、軌道パラメータｓの下記（１７）式を導出する。

トルクの上下限制約条件(ＭＶＣ)を示す上記（１７）式は、上記（１）式に示す最短時間制御の制約条件の標準形となる。但し、上記（１７）において、ａ（ｓ）、ｂ（ｓ）、ｃ（ｓ）、ｄ（ｓ）は、軌道パラメータｓで表される係数である。

このように、制約条件算出部９１は、運動方程式（１３）式と、予め設定された全方位移動体１が移動すべき軌道であって軌道パラメータｓを含む指令軌道ｆ（ｓ）と、に基づいて、駆動輪２２及び旋回軸４のトルクを制約する条件であって、軌道パラメータで表現されたトルク制約条件（ＭＶＣ）を示す式（１７）を算出する。

次に、全方位移動体１の駆動輪２２及び旋回軸４の回転速度の制約条件(ＶＬＣ)について説明する。上述の如く、駆動輪２２及び旋回軸４を駆動する車輪及び旋回アクチュエータ７、８の出力性能を考慮して、駆動輪２２及び旋回軸４の速度に制約条件を付加する必要がある。

駆動輪２２の速度の制約を下記式により設定する。

旋回軸４の速度の制約を下記式により設定する。

だだし、ω_jointは旋回軸４の回転速度、ω_joint、minおよびω_joint、maxはそれぞれ旋回軸４の速度の上限値および下限値、ω_l、minおよびω_l、maxはそれぞれ左駆動輪２２の速度の下限値および上限値、ω_ｒ、minおよびω_ｒ、maxはそれぞれ右駆動輪２２の速度の下限値および上限値である。
上記（１５）式と逆運動学の結果から、上記（２０）、（２１）、（２２）式の不等式制約は、以下のように軌道パラメータ（ｓドット）に関する不等式制約に変形することができる。

ただし、ｓ（ドット）_l、max、ｓ（ドット）_ｒ、max、ｓ（ドット）_joint、maxは、それぞれ左駆動輪２２、右駆動輪２２、旋回軸４に関する、式変形後のｓ（ドット）の最大値である。上記（２３）式を用いて、駆動輪２２及び旋回軸４の回転速度の制約条件を表す曲線ＶＬＣが算出される。
このように、制約条件算出部９１は、予め設定された駆動輪２２及び旋回軸４の速度の制約（２０）、（２１）、（２２）式に基づいて、駆動輪２２及び旋回軸４の速度を制約する条件であって、軌道パラメータｓで表現された速度制約条件（ＶＬＣ）を上記不等式（２３）より算出する。

軌道算出部９２は、制約条件算出部９１により算出されたＭＶＣ及びＶＬＣ曲線の下を通過しながら、軌道上の各時刻では加速度の最大値もしくは最小値で加減速する、軌道パラメータｓ−ｓ（ドット）平面上の最短時間軌道を算出する。

ここで、最短時間軌道の算出方法の一例について詳細に説明する。
軌道算出部９２は、軌道パラメータｓ−ｓ（ドット）平面における始点（ｓ＝０）および終点（ｓ＝１）から夫々出発して、所定の積分幅で積分を繰り返して最短時間軌道を算出する（図４）。ここで、所定の積分幅は任意に設定可能であるが、実際のサンプリング周期より小さいのが好ましい（後に、補間を行うことも可能である）。

始点側から終点側へ前進で積分（以下、前進積分）を行う場合は、最大加速度で積分を行う。終点側から始点側へ後退で積分（以下、後退積分）を行う場合は、最小加速度で積分を行う。軌道算出部９２は、上記規則に基づいて前進積分及び後退積分を繰返し最短時間軌道を生成する。

軌道算出部９２は、前進積分において、ＶＬＣ曲線と交差する際、ＶＬＣ曲線に沿える場合はＶＬＣ曲線に沿って前進積分を進める（ステップＳ１）。これは、駆動輪２２及び旋回軸４の最大速度での動作区間に相当する。

一方、前進積分において、ＶＬＣ曲線と交差する際、このＶＬＣ曲線に沿えない場合、あるいはＭＶＣ曲線と交差する場合は、実行不可能な軌道となる。この場合、軌道算出部９２は、スイッチングポイントの候補を探索する（ステップＳ２）。

ここで、スイッチングポイントとは、最小加速度から最大加速度へと切り替わる点であり、この点を通るとＭＶＣ曲線の下を通過する軌道を生成することができる。なお、スイッチングポイントの候補として、例えば、以下の点が知られている。
・ＭＶＣ曲線上での加速度の方向が、ＭＶＣ曲線自体の接線と一致する点(tangent switching point)。
・ＭＶＣ曲線自体が不連続に変化する点(discontinuous switching point)。
・トルクの制約条件の項 a(s)がゼロとなる点(zero inertia switching point)。

軌道算出部９２は、探索したスイッチングポイントの候補を始点として、最小加速度で後退積分を行う（ステップＳ３）。

軌道算出部９２は、この後退積分の軌道がこれまで前進積分で生成した軌道と矛盾無く繋がる場合、このスイッチングポイントの候補を始点として前進積分を再開する。一方で、軌道算出部９２は、この後退積分の軌道がこれまで前進積分で生成した軌道と矛盾して繋がらない場合、例えば、次の（図４の右側にある）スイッチングポイントの候補を探索する。そして、軌道算出部９２は、この次のスイッチングポイントの候補を始点とした後退積分の軌道がこれまで前進積分で生成した軌道と矛盾無く繋がるかを判定する。軌道算出部９２は、探索したスイッチングポイントの候補を始点とした後退積分の軌道がこれまで前進積分で生成した軌道と矛盾無く繋がるまで、スイッチングポイントの候補の探索を繰り返す（ステップＳ４）。

軌道算出部９２は、上記（ステップＳ１）乃至（ステップＳ４）を繰返し、前進積分の軌道と後退積分の軌道とを接続した最短時間軌道（例えば、図４に示すTime optimal trajectory、ｓ（ドット）＝ｇ（ｓ））を算出する。このように、軌道算出部９２は、算出されたトルク制約条件ＭＶＣおよび速度制約条件ＶＬＣに基づいて、軌道パラメータｓ−ｓ（ドット）平面における始点および終点から所定の積分幅で前進積分及び後退積分を繰り返して最小加速度から最大加速度へと切り替わるスイッチングポイントを探索することで最短時間軌道（ｓ（ドット）＝ｇ（ｓ））を算出する。

制御部９３は、軌道算出部９２により算出された最短時間軌道(ｓ（ドット）＝ｇ（ｓ））に基づいて、車輪アクチュエータ７および旋回アクチュエータ８を制御する。
制御部９３は、最短時間軌道(ｓ（ドット）＝ｇ（ｓ））に基づいて、全方位移動体１の本体部３の位置姿勢の目標値ｑ（ｔ）を算出する。制御部９３は、算出した全方位移動体１の本体部３の位置姿勢の目標値ｑ（ｔ）に基づいて、車輪アクチュエータ７および旋回アクチュエータ８を制御する。

ここで、全方位移動体１の本体部３の位置姿勢の目標値ｑ（ｔ）の算出方法の一例について、詳細に説明する。
まず、制御部９３は、初期値ｉ＝０、ｓｉ＝０を設定する（ステップＳ１１）。
制御部９３は、時間ｔｉにおけるｑｉ＝ｆ（ｓｉ）を算出する（ステップＳ１２）。
制御部９３は、サンプル時間Δｔ時間進んだ際のｓｉを下記式で算出する（ステップＳ１３）。
ｓｉ＋１＝ｓｉ＋ｇ（ｓｉ）＊Δｔ
ｔｉ＋１＝ｔｉ＋Δｔ

制御部９３は、ｓｉ≦１であるか否かを判定する（ステップＳ１４）。
制御部９３は、ｓｉ≦１であると判定したとき（ステップＳ１４のＹＥＳ）、全方位移動体１の本体部３の位置姿勢の目標値ｑ（ｔ）の算出処理を終了する。一方、制御部９３は、ｓｉ≦１でないと判定したとき（ステップＳ１４のＮＯ）、ｉをインクリメントし（ｉ＝ｉ＋１）（ステップＳ１５）、上記（ステップＳ１３）に戻る。

制御部９３は、上記算出したｑｉ及びｔｉの組合せを、全方位移動体１の本体部３の位置姿勢の目標値ｑ（ｔ）とする。なお、制御部９３は、上記（ステップＳ１３）において、オイラー積分を用いてｓｉを算出しているが、これに限定されない。制御部９３は、上記（ステップＳ１３）において、台形積分を用いてｓｉを算出してもよい。これにより、より高精度にｓｉを算出でき、全方位移動体１の本体部３の位置姿勢の目標値ｑ（ｔ）をより高精度に算出できる。

図６は、本実施形態に係る全方位移動体の制御方法のフローを示すフローチャートである。
制約条件算出部９１は、台車部２及び本体部３の重心及びｚ軸周りに関する運動方程式（７）式乃至（１０）式に基づいて、慣性項と、遠心力及びコリオリ項と、粘性摩擦項と、重力項と、を含む駆動輪２２及び旋回軸４のトルクに関する運動方程式（１３）式を算出する。さらに、制約条件算出部９１は、この運動方程式（１３）式と、予め設定された全方位移動体１が移動すべき軌道であって軌道パラメータｓで表現された指令軌道ｆ（ｓ）と、に基づいて、駆動輪２２及び旋回軸４のトルクを制約する条件であって、軌道パラメータｓで表現されたトルク制約条件（ＭＶＣ）を示す式（１７）を算出する（ステップＳ１０１）。

制約条件算出部９１は、予め設定された駆動輪２２及び旋回軸４の速度の制約（２０）式乃至（２２）式に基づいて、駆動輪２２及び旋回軸４の速度を制約する条件であって、軌道パラメータｓで表現された速度制約条件（ＶＬＣ）を示す式（１９）を算出する（ステップＳ１０２）。

軌道算出部９２は、制約条件算出部９１により算出されたトルク制約条件および速度制約条件に基づいて、軌道パラメータｓ−ｓ（ドット）平面における始点（ｓ＝０）および終点（ｓ＝１）から所定の積分幅で前進積分及び後退積分を繰り返してスイッチングポイントを探索することで、最短時間軌道を算出する（ステップＳ１０３）。

制御部９３は、軌道算出部９２により算出された最短時間軌道(ｓ（ドット）＝ｇ（ｓ））に基づいて、全方位移動体１の本体部３の位置姿勢の目標値ｑ（ｔ）を算出する（ステップＳ１０４）。制御部９３は、算出した全方位移動体１の本体部３の位置姿勢の目標値ｑ（ｔ）に基づいて、車輪アクチュエータ７および旋回アクチュエータ８を制御することで、台車部２の駆動輪２２および本体部３の旋回軸４を制御する（ステップＳ１０５）。

次に、駆動輪２２及び旋回軸４の駆動トルク及び回転速度の制約条件を付加し、設定された指令軌道に最短時間で追従する最短時間軌道の生成を行ったシミュレーション結果について説明する。

ここで、最短時間軌道生成のアルゴリズムは、例えば、Python及びC言語で生成し、その動作確認を行っている。本シミュレーションでは、図７に示す指令軌道を用いている。この指令軌道は、全方位移動体１の初期姿勢の前方向をｘ、左方向をｙとした世界座標系において、（０、０）、（１、−１）、（１、１）、（−１、−１）、（−１、１）、（０、０）を経由点とした３次自然スプライン補間で生成されている。全方位移動体１の本体部３の向きは常に角度０を維持するように指示されている。左右駆動輪２２の速度の上限値１８ｒａｄ／ｓとし、左右駆動輪２２のトルクの上限値を０．３Ｎｍとしている。旋回軸４の速度の上限値は、３．０ｒａｄ／ｓとし、旋回軸４のトルクの上限値は、０．２Ｎｍとしている。

図８は、左駆動輪の駆動トルクを示す図である。図９は、右駆動輪の駆動トルクを示す図である。図１０は、旋回軸の駆動トルクを示す図である。なお、図８乃至１０において、破線は各駆動トルクの上下限値を示している。図８乃至１０に示す如く、左右駆動輪２２および旋回軸４の駆動トルクは、上下限値の制約内で推移していることが分かる。したがって、本シミュレーションにおいて、左右駆動輪２２および旋回軸４の駆動トルクが正しく制約されていることが分かる。

図１１は、図８乃至１０に示すように左右駆動輪および旋回軸の駆動トルクが制約されたときの、軌道パラメータｓ−ｓ（ドット）平面上の軌道を示す図である。最短時間制御は、軌道上各点において、最大加速度もしくは最小加速度を取るアルゴリズであるため、得られた軌道は、最短時間軌道となる。

以上、本実施形態において、台車部２及び本体部３の重心及び鉛直方向軸周りに関する運動方程式に基づいて、慣性項と、遠心力及びコリオリ項と、粘性摩擦項と、重力項と、を含む駆動輪２２及び旋回軸４のトルクに関する運動方程式が算出され、該運動方程式と、予め設定された当該全方位移動体１が移動すべき軌道であって軌道パラメータｓで表現された指令軌道ｆ（ｓ）と、に基づいて、駆動輪２２及び旋回軸４のトルクを制約する条件であって、軌道パラメータで表現されたトルク制約条件ＭＶＣが算出される。また、予め設定された駆動輪２２及び旋回軸４の速度の制約に基づいて、駆動輪２２及び旋回軸４の速度を制約する条件であって、軌道パラメータｓで表現された速度制約条件ＶＬＣが算出される。算出されたトルク制約条件ＭＶＣおよび速度制約条件ＶＬＣを満たし、かつ、指令軌道ｆ（ｓ）を最短時間で追従する軌道であって、軌道パラメータｓ−ｓ（ドット）平面上に表現された最短時間軌道が算出される。制御装置９の制御部９３は、算出された最短時間軌道に基づいて本体部３の位置姿勢の目標値を算出する。そして、制御装置９の制御部９３は、該算出した本体部３の位置姿勢の目標値に基づいて車輪及び旋回アクチュエータ７、８を制御する。
これにより、上記静摩擦やＺＭＰのような指標で律速させる特異な点を含めてアクチュエータの出力性能を考慮しつつ、最短時間での最適な制御を行うことができる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
例えば、上記実施形態において、全方位移動体１の制御装置９が制約条件算出部９１と、軌道算出部９２とを有する構成であるが、これに限定されない。制約条件算出部９１及び軌道算出部９２のうちの少なくとも一方が、全方位移動体１とは別の装置に設けられる構成であってもよい。この場合、別の装置の制約条件算出部９１で算出されたトルク制約条件及び速度制約条件あるいは、軌道算出部９２により算出された最短時間軌道は、全方位移動体１の制御装置９に送信、あるいはユーザによって全方位移動体１の制御装置９に入力されてもよい。

上記実施形態において、軌道算出部９２は、ＭＶＣ及びＶＬＣ曲線の下を通過しながら、空間軌道を追従できるような駆動輪２２及び旋回軸４の加速度のうちのいずれかが、最大加速度もしくは最小加速度になっているような最短時間軌道を算出しているが、これに限定されない。軌道算出部９２は、例えば、ＭＶＣ及びＶＬＣ曲線の下を通過しながら、最短でない（遅い）実現可能な軌道を算出してもよい。

上記最短時間制御による、最大加速度もしくは最小加速度になっているような最短時間軌道においては、加速度が不連続となる。このような加速度の不連続状態が好ましくない場合、軌道算出部９２は、例えば、ＭＶＣ及びＶＬＣ曲線の下を通過しながら、滑らかで（軌道上の各時刻の加速度が連続的となるような）実現可能な軌道を算出してもよい（図１２）。これにより、上記静摩擦やＺＭＰのような指標で律速させる特異な点を含めてアクチュエータの出力性能を考慮しつつ、最適な制御を行うことができる。

また、本発明は、例えば、図６に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。

また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１ 全方位移動体、２ 台車部、３ 本体部、４ 旋回軸、５ 回転センサ、６ 回転センサ、７ 車輪アクチュエータ、８ 旋回アクチュエータ、９ 制御装置、２１ 補助輪、２２ 駆動輪、９１ 制約条件算出部、９２ 軌道算出部、９３ 制御部

Claims (5)

1. 複数の駆動輪が設けられた台車部と、台車部に旋回軸を介して回転可能に設けられた本体部と、前記駆動輪を駆動する第１駆動手段と、前記旋回軸を駆動する第２駆動手段と、を備え、全方位に移動可能な全方位移動体であって、
   前記台車部及び本体部の重心及び鉛直方向軸周りに関する運動方程式に基づいて、慣性項と、遠心力及びコリオリ項と、粘性摩擦項と、重力項と、を含む前記駆動輪及び旋回軸のトルクに関する運動方程式が算出され、該運動方程式と、予め設定された当該全方位移動体が移動すべき軌道であって軌道パラメータで表現された指令軌道と、に基づいて、前記駆動輪及び旋回軸のトルクを制約する条件であって、前記軌道パラメータで表現されたトルク制約条件が算出され、
   予め設定された前記駆動輪及び旋回軸の速度の制約に基づいて、前記駆動輪及び旋回軸の速度を制約する条件であって、前記軌道パラメータで表現された速度制約条件が算出され、
   前記算出されたトルク制約条件および速度制約条件を満たし、かつ、前記指令軌道を追従する軌道であって、前記軌道パラメータ及びその微分値の平面上に表現された前記軌道が算出され、
   前記算出された軌道に基づいて前記本体部の位置姿勢の目標値を算出し、該算出した本体部の位置姿勢の目標値に基づいて前記第１及び第２駆動手段を制御する制御手段を備える、
   ことを特徴する全方位移動体。
2. 請求項１記載の全方位移動体であって、
   前記算出された軌道は、前記算出されたトルク制約条件および速度制約条件を満たし、かつ、前記指令軌道を最短時間で追従する最短時間軌道である、ことを特徴とする全方位移動体。
3. 請求項２記載の全方位移動体であって、
   前記算出された軌道は、前記算出されたトルク制約条件および速度制約条件に基づいて、軌道パラメータ及びその微分値の平面における始点および終点から所定の積分幅で前進積分及び後退積分を繰り返して最小加速度から最大加速度へと切り替わるスイッチングポイントを探索することで算出された前記最短時間軌道である、
   ことを特徴とする全方位移動体。
4. 複数の駆動輪が設けられた台車部と、台車部に旋回軸を介して回転可能に設けられた本体部と、前記駆動輪を駆動する第１駆動手段と、前記旋回軸を駆動する第２駆動手段と、を備え、全方位に移動可能な全方位移動体の制御方法であって、
   前記台車部及び本体部の重心及び鉛直方向軸周りに関する運動方程式に基づいて、慣性項と、遠心力及びコリオリ項と、粘性摩擦項と、重力項と、を含む前記駆動輪及び旋回軸のトルクに関する運動方程式を算出し、該運動方程式と、予め設定された当該全方位移動体が移動すべき軌道であって軌道パラメータで表現された指令軌道と、に基づいて、前記駆動輪及び旋回軸のトルクを制約する条件であって、前記軌道パラメータで表現されたトルク制約条件を算出するステップと、
   予め設定された前記駆動輪及び旋回軸の速度の制約に基づいて、前記駆動輪及び旋回軸の速度を制約する条件であって、前記軌道パラメータで表現された速度制約条件を算出するステップと、
   前記算出されたトルク制約条件および速度制約条件を満たし、かつ、前記指令軌道を追従する軌道であって、前記軌道パラメータ及びその微分値の平面上に表現された前記軌道を算出するステップと、
   前記算出した軌道に基づいて前記本体部の位置姿勢の目標値を算出し、該算出した本体部の位置姿勢の目標値に基づいて前記第１及び第２駆動手段を制御するステップと、
   を含む、ことを特徴する全方位移動体の制御方法。
5. 複数の駆動輪が設けられた台車部と、台車部に旋回軸を介して回転可能に設けられた本体部と、前記駆動輪を駆動する第１駆動手段と、前記旋回軸を駆動する第２駆動手段と、を備え、全方位に移動可能な全方位移動体のプログラムであって、
   前記台車部及び本体部の重心及び鉛直方向軸周りに関する運動方程式に基づいて、慣性項と、遠心力及びコリオリ項と、粘性摩擦項と、重力項と、を含む前記駆動輪及び旋回軸のトルクに関する運動方程式を算出し、該運動方程式と、予め設定された当該全方位移動体が移動すべき軌道であって軌道パラメータで表現された指令軌道と、に基づいて、前記駆動輪及び旋回軸のトルクを制約する条件であって、前記軌道パラメータで表現されたトルク制約条件を算出する処理と、
   予め設定された前記駆動輪及び旋回軸の速度の制約に基づいて、前記駆動輪及び旋回軸の速度を制約する条件であって、前記軌道パラメータで表現された速度制約条件を算出し、
   前記算出されたトルク制約条件および速度制約条件を満たし、かつ、前記指令軌道を追従する軌道であって、前記軌道パラメータ及びその微分値の平面上に表現された前記軌道を算出する処理と、
   前記算出した軌道に基づいて前記本体部の位置姿勢の目標値を算出し、該算出した本体部の位置姿勢の目標値に基づいて前記第１及び第２駆動手段を制御する処理と、
   をコンピュータに実行させる、
   ことを特徴するプログラム。

Images