JP5712643B2 - 移動体 - Google Patents

Description

本発明は、移動体に関する。

近年、定められた軌道上を自律的に移動する移動体が開発されている。移動体が、空間にあらかじめ設定された軌道上を追従したい場合には、移動体の位置を軌道上に合わせ、移動体の方向を軌道の方向に合わせる必要がある。

ここで、移動体が、フィードフォワード制御により軌道を追従する場合には、路面の凹凸による台車のバンピングや、車輪のスリップ等による軌道からのずれが発生する。したがって、移動体は、センサで移動体の位置と方向を計測し、軌道に戻るよう移動体の進行方向の速度や、左右方向の回転速度を補正する必要がある。

さらに、移動体は、狭い場所を通過するような場合には、軌道をオーバーシュートすることなく、かつ速やかに軌道上に収束することが要求される。

特許文献１には、床面から受ける走行抵抗や横滑力の変化に対して、これらを打ち消す目標変位を設定する移動体について記載されている。これによると、移動体は、旋回方向に対しては移動体の方位変位に一致させるようにフィードバック制御を行い、並進方向に対してはフィードフォワード制御を行う。

特許文献２には、安定限界速度を上回る場合において、旋回方向に対してフィードバック制御を行い、フィードバック操作量を設定する移動体について記載されている。これによると、移動体は、設定したフィードバック操作量の限界量に対し、安全にフィードバックできる安定化フィードバック操作の代を残して、フィードフォワード量を設定する。

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

しかしながら、移動体は、軌道に接近する場合に軌道にうまく乗ることができず、オーバーシュートや振動が発生することがある。また、移動体は、フィードバック制御により進行方向の速度が変化すると、進行方向の速度変化が滑らかでなくなる。さらに、移動体は、車輪に過大な速度が与えられた場合に、車輪の１つが回転速度のリミットで制限されると、軌道から外れてしまうことがある。
本発明にかかる移動体は、このような問題点を解決するためになされたものであり、設定された軌道を適切に走行する移動体を提供する。

本発明にかかる移動体は、駆動輪を有する移動体であって、現在の位置及び方向を取得する位置情報取得手段と、前記位置情報取得手段により取得された位置情報および移動体の軌道の情報を記憶する記憶手段と、制御手段と、を有し、前記制御手段は、距離偏差と角度偏差を求め、駆動輪の回転速度と距離偏差および角度偏差の時間変化との関係を算出する演算手段と、前記移動体の並進速度としてフィードフォワード値を与える設定手段と、を備え、前記距離偏差と、前記角度偏差と、前記距離偏差のゲインと同一のゲインを用いた距離偏差の時間変化と、を用いてフィードバック制御を行う。
これにより、ゲインおよび速度の影響によって軌道をオーバーシュートさせることなく、移動体を滑らかに走行させることができる。

移動体は、設定された軌道を適切に走行する。

実施の形態１にかかる移動体のブロック図である。 実施の形態１にかかる移動体における処理動作を示す図である。 実施の形態１にかかる座標系の図である。 実施の形態１にかかる移動体が微小変化するときの図である。 実施の形態１にかかる移動体および駆動輪のアライメントを示す図である。 実施の形態１にかかるコントローラへの入力と移動体１０および軌道の関係を示す図である。 実施の形態１にかかる移動体の距離偏差の増減を示す図である。 実施の形態１にかかる移動体が軌道と平行に走行する状態の図である。 実施の形態１にかかる移動体の角度偏差と回転方向の関係を示す図である。 実施の形態１にかかる移動体をフィードバック制御した場合の旋回方向を示す図である。 実施の形態１にかかる移動体の距離偏差と角度偏差の関係を示す図である。 実施の形態１にかかる移動体の距離偏差と角度偏差の関係を示す図である。 実施の形態１にかかる移動体の距離偏差と角度偏差の関係を示す図である。 実施の形態１にかかる移動体の左右の車輪速度の制限の図である。

実施の形態１
以下、図面を参照して本発明の実施の形態について説明する。図１は、本発明にかかる移動体のブロック図である。

移動体１０は、位置情報取得手段１１０と、記憶手段１２０と、駆動輪１３０と、制御手段１４０と、を備える。

位置情報取得手段１１０は、移動体１０に設けられたセンサである。位置情報取得手段１１０は、移動体１０の位置情報と、移動体１０が向いている方向の情報を取得する。
位置情報取得手段１１０は、取得した移動体１０の位置情報と方向情報を、記憶手段１２０に出力する。

記憶手段１２０は、位置情報取得手段１１０から供給される位置情報及び方向情報と、移動体１０が移動する軌道の情報を記憶する。典型的には、記憶手段１２０は、メモリやＨＤＤであるが、記憶媒体はこれらに限られない。記憶手段１２０は、軌道の情報として、軌道の経路とともに軌道の曲率を記憶する。
記憶手段１２０は、記憶した位置情報及び方向情報と、軌道の情報を、制御手段１４０に出力する。

駆動輪１３０は、移動体１０に複数設けられた移動手段である。駆動輪１３０は、制御手段１４０により駆動制御された駆動手段１３１が動作することにより回転駆動する。移動体１０は、駆動輪１３０が回転駆動することにより、前後方向への移動や、回転、旋回動作を行う。
なお、駆動輪１３０は、移動体１０の左右に同軸上に配置されているものとする。以下では、駆動輪１３０を車輪と呼ぶこともある。

駆動手段１３１は、制御手段１４０から制御信号を受けて駆動する。駆動手段１３１は、典型的にはモータである。駆動手段１３１は、制御信号に基づいて駆動輪１３０に駆動力を与える。

制御手段１４０は、演算手段１４１と、設定手段１４２と、を備える。制御手段１４０は、移動体１０の走行状態の制御を行う。より具体的には、制御手段１４０は、移動体１０の旋回速度について、距離偏差と角度偏差と、距離偏差の時間変化に基づいてフィードバック制御する。このとき、制御手段１４０は、距離偏差の時間変化のゲインを、距離偏差のゲインと同じにする。後に詳述する。
制御手段１４０には、ユーザにより入力された駆動輪１３０の回転速度（車輪速度）と、記憶手段１２０が記憶している情報が供給される。

演算手段１４１は、典型的にはＣＰＵ（Central Processing Unit）を有し、移動体１０が軌道を走行するための演算処理を行う。また、演算手段１４１は、一時的に情報を記憶するためのＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）などを備えていても良い。
演算手段１４１は、記憶手段１２０から、移動体１０の位置情報、方向情報、軌道情報が供給を受け、移動体１０から軌道までの最短距離（距離偏差）と、移動体１０と軌道の方向の差（角度偏差）を求める。また、演算手段１４１は、車輪速度と、距離偏差、角度偏差の時間変化との関係を算出する。
さらに、演算手段１４１は、移動体１０の旋回速度にかかるフィードバック制御のための演算を行う。制御手段１４０は、演算手段１４１により演算された値に基づいて、駆動手段１３１に制御信号を出力する。これにより、駆動輪１３０は、移動体１０が旋回するよう動作する。

設定手段１４２は、移動体１０の軌道との並進方向の速度として、ユーザが任意の値（フィードフォワード値）を設定するための設定手段である。制御手段１４０は、設定手段１４２で設定されたフィードフォワード値に基づいて、駆動手段１３１に制御信号を出力する。これにより、駆動輪１３０は、移動体１０が軌道の並進方向に走行するよう動作する。以下では設定手段１４２をコントローラと記述することがある。

次に、移動体１０の動作について説明する。以下では、移動体１０と軌道とを一体のシステムとして扱うことがある。
図２は、移動体１０が、フィードフォワード制御およびフィードバック制御により走行する状態を示す図である。
ユーザは、移動体１０に車輪速度を入力する。移動体１０は、軌道までの最短距離と、移動体１０の向きと軌道のなす角を出力する。また、移動体１０は、車輪速度、軌道までの最短距離となす角の時間微分の関係をヤコビアンで表現する。移動体１０のコントローラは、入力された指示を、逆ヤコビアンの形式にする。
移動体１０の並進速度は、フィードフォワード制御に基づいて設定する。移動体１０の旋回速度は、軌道までの最短距離となす角に応じて、距離偏差と角度偏差が収束するようにフィードバック制御を行うことにより設定する。

さらに、移動体１０の車輪速度は、ハードウェア、ソフトウェアで上限があるが、移動体１０の左右に設けた駆動輪１３０のどちらか一方が上限にかかり、指令値どおりに回転しない場合には軌道追従性が悪化するため、上限を超えず、軌道に追従するように車輪速度を補正する。
以下、詳細について説明する。

最初に、演算手段１４１による、車輪速度と並進速度、角度偏差の微分値の関係の算出について説明する。演算手段１４１は、移動体１０の並進速度、回転速度と距離偏差、角度偏差の微分値の関係を求め、次に車輪速度と並進速度、回転速度の関係を求める。

まず、移動体１０の座標系を定義する。ここで、移動体１０と軌道の位置関係は、移動体１０を基準として定める。回転の方向は、反時計回りを正と定める。図３は、移動体１０と軌道の位置関係を示す図である。
図３に示したように、移動体１０から軌道までの最短距離を、距離偏差とする。最短距離は、移動体１０の位置と軌道上の最近傍点を結んだ線分の長さとし、最近傍点での軌道の接線と直角であるとする。符号は、移動体１０から見た軌跡までのベクトルをｐ_ｒ、軌道が進む方向の接線ベクトルをｐ_ｓとすると、外積ｐ_ｒ×ｐ_ｓのｚ成分の符号とする。
角度偏差は、移動体１０から見た軌道の角度とする。図３の場合、角度偏差は正、距離偏差も正である。演算手段１４１は、この座標系に基づいて演算を行う。

演算手段１４１は、移動体１０の並進速度、回転速度と距離偏差、角度偏差の微分値の関係を求める。
図４は、移動体１０が微小変化した場合の図である。図４（ａ）は、移動体１０の距離偏差の微小変化にかかる図であり、図４（ｂ）は、移動体１０の角度偏差の微小変化にかかる図である。
移動体１０の移動方向に沿った微小変異をｄ_ｓ、移動体１０の微小回転角度をｄθ_ｒ、軌道の曲率半径をρ_ｓ、軌道の微小回転ｄθ_ｓとすると、幾何学的に以下の関係が成り立つ。
距離偏差ｌの変化ｄｌは、

である。角度偏差θの変化ｄθは、

である。また、以下の関係が成り立っている。

式（１．２）、（１．３）からｄθ_ｓを消去すると、

である。式（１．１）、（１．４）の微小時間ｄｔにおける変化は、

である。ここで、ｄｓ／ｄｔは移動体１０の並進速度、ｄθ_ｒ／ｄｔは移動体１０の回転速度であるから、これらをそれぞれｖ_ｒ、ω_ｒとおくと、

である。式（１．７）と式（１．８）を行列表現すると、

と表現できる。ここで、表記の簡単化のため、

として、

と表記する。

次に、演算手段１４１は、移動体１０の車輪速度と、移動体１０の並進速度、回転速度の関係を求める。図５は、移動体１０および駆動輪１３０のアライメントを示す図である。図５（ａ）は、移動体１０の平面図であり、図５（ｂ）は、移動体１０の正面図である。
移動体１０の車輪間距離Ｗ、車輪半径ｒとすると、運動学的に式（１．１１）の関係が成り立つ。

ここで、

は、移動体１０の右側に備えられた駆動輪１３０の回転速度であり、

は、移動体１０の左側に備えられた駆動輪１３０の回転速度である。
表記の簡単化のため、

として、

と表記する。
式（１．１０）と、式（１．１２）から、車輪速度と距離偏差、角度偏差の微分値の関係は、式（１．１３）となる。

次に、移動体１０への速度を指令するコントローラに対し、ユーザが入力を行った場合について説明する。ここでコントローラの操作による移動体１０の動作制御は、通常のヤコビアンの逆行列の制御と同様である。演算手段１４１は、以下の演算を行う。

ユーザによるコントローラへの入力を、ｕ＝[ｕ_ｌ，ｕ_θ]^Ｔとする。ここで、移動体１０に与えられる車輪速度は、式（１．１３）の逆行列とする。すなわち、

とする。ここで、

である。Ｊ_ｒの逆行列は、

であり、Ｊ_ｅの逆行列は、

である。コントローラは、式（１．１３）の行列の逆行列なので、ｕ＝[ｕ_ｌ，ｕ_θ]^Ｔで移動体１０を動かしたとき、距離偏差、角度偏差の微分値は以下のようにｕと等価である。

図６は、コントローラへの入力ｕ_ｌと、移動体１０および軌道の関係を表す図である。図６のように、移動体１０が、軌道に対して角度θ、指令速度ｖ_ｒｅｆで移動している状態において、距離偏差の微分値は、ｄｌ／ｄｔ＝−ｖ_ｒｅｆｓｉｎθである。このようなｄｌ／ｄｔを発生させるｕ_ｌは、式（２．５）よりｕ_ｌ＝ｄｌ／ｄｔであるので、ｕ_ｌを以下のようにする。

移動体１０への指令速度ｖ^＊は、入力[−ｖ_ｒｅｆｓｉｎθ，ｕ_θ]^Ｔを、式（２．４）に与えることで得られる。すなわち、

となる。
ここで、距離偏差、角度偏差が０のときｕ_θ＝０となるようにｕ_θを決めれば、ω^＊＝ｖ_ｒｅｆ／ρ_ｓとなり、軌道の曲率に追従するｖ^＊とω^＊になる。
式（２．７）のｖ^＊とω^＊を、移動体１０に与えると、式（１．９）より

となる。ｄｌ／ｄｔはθの値によって正にも負にもなるので、必ずしもｌが０に近づくとは限らない。図７は、距離偏差の増減を示した図である。図７（ａ）に示すように、ｌ＞０かつθ＞０の場合には、ｄｌ／ｄｔ＜０であり、ｌは減少する。図７（ｂ）に示すように、ｌ＞０かつθ＜０の場合には、ｄｌ／ｄｔ＞０であり、ｌは増加する。図７（ｃ）に示すように、ｌ＜０かつθ＜０の場合には、ｄｌ／ｄｔ＞０であり、ｌは減少する。図７（ｄ）に示すように、ｌ＜０かつθ＞０の場合には、ｄｌ／ｄｔ＜０であり、ｌは増加する。

次に、旋回速度入力ｕ_θを決定する。ここで、ｕ_θは、ｌとθの関数とする。すなわち、

である。しかしながら、ｕ_θを角度偏差だけの関数にすると、軌道と移動体１０の方向が同じ場合に、移動体１０は回転せず、距離偏差が減少しない。図８に、移動体１０が軌道と平行に走行している状態を示す。図８のように、移動体１０は、角度偏差が無くとも距離偏差があるときには回転させる必要がある。
図９は、移動体１０の角度偏差と回転方向の関係を示した図である。

したがって、ｆ（ｌ，θ）について、ｆ（０，０）＝０、かつ、図９（ａ）のように移動体１０が軌道に接近する場合にはｄθ／ｄｔ＜０とし、図９（ｂ）のように移動体１０が軌道から遠ざかる場合にはｄθ／ｄｔ＞０となるようにする。
これらの条件を満たすひとつの形は

である。ここでαとβはゲインであり、正の値とする。

図１０は、移動体１０が、式（２．１０）のフィードバック制御により走行する場合について、定性的に示した図である。
θ＞０かつθ＞βｌであれば、移動体１０は、図１０（ａ）のように設定された軌道に近づきつつ、角度偏差が小さくなるように移動する。これを状態ａとする。
θ＞０かつθ＝βｌであれば、移動体１０は、図１０（ｂ）のように設定された軌道に直線的に近付くよう移動する。これを状態ｂとする。
θ＞０かつθ＜βｌであれば、移動体１０は、図１０（ｃ）のように軌道に対して、角度偏差が大きくなるように移動する。これを状態ｃとする。
θ＝０であれば、移動体１０は、図１０（ｄ）のように、軌道と並列して走行している状態から、軌道に近づくように移動する。これを状態ｄとする。
θ＜０であれば、移動体１０は、図１０（ｅ）のように、軌道から遠ざかる方向に走行した状態から向きを変え、軌道に近づく方向に走行するように移動する。これを状態ｅとする。

図１１は、ｌ≧０のときのｌとθの軌跡を示した図である。図１１に記載された状態ａ〜ｅは、図１０の状態ａ〜ｅに対応している。なお、ｌ≦０の場合には、原点に対して点対象の軌跡となる。
状態ｂから状態ａにいたる過程で、ｄｌ／ｄｔに対して、ｄθ／ｄｔが小さい場合には、図１１のパターン１の軌跡をたどりｌ≦０のサイクルに入るため、移動体１０は振動することになる。また、初期値θ_ｓの絶対値より最終値θ_ｅの絶対値が大きい場合には、サイクルを繰り返していくうちに振幅が大きくなり、発散振動する。逆にｄｌ／ｄｔに対してｄθ／ｄｔが大きい場合には、θ＝βｌに沿って、ｌ＝０、θ＝０に収束する。
βが大きい場合には、状態ｂになる角度θが大きくなり、移動体１０が軌道に対して角度を持って突っ込むことになるので、パターン１の軌跡になりやすい。逆にβを小さくすると、状態ｂの角度が小さくなるので、ｌ＝０になるまでの収束が遅い。
αが大きい場合には、ｄθ／ｄｔが大きくなるので、パターン２の軌跡となる。

次に、微分値のフィードバックについて説明する。
移動体１０が軌道に近づく際に発生する振動は、距離偏差の変化が角度偏差に対して相対的に大きいことにより、図１１のパターン１の軌道になるためである。
そこで、演算手段１４１は、距離偏差の変化に応じて角度偏差の変化を変化させるよう演算を行う。

距離偏差の速度変化は−ｖ_ｒｅｆｓｉｎθであるから、距離偏差のゲインとして用いられているゲインβを利用し、ｕ_θを以下のようにする。

図１２は、−βｖ_ｒｅｆｓｉｎθの作用について示した図である。図１２に示すようにｌ−θ軌道と、直線βｌの交点では、ｄｌ／ｄｔ＝ｖ_ｒｅｆｓｉｎθであるので、ｌ−θ軌道はちょうど直線に沿う方向に変化することになる。したがって、移動体１０の軌道は、θ＝βｌの直線を越えることは無い。

また、式（２．１１）が０となる直線は、ｓｉｎθが線形とみなせる範囲でθ＝β・ｌ・α／（α＋βｖ_ｒｅｆ）である。図１３は、θ＝β・ｌ・α／（α＋βｖ_ｒｅｆ）の作用について示した図である。図１３において、θ＝β・ｌ・α／（α＋βｖ_ｒｅｆ）は、必ずβｌよりも下側である。さらに、この直線上ではｄｌ／ｄｔ＜０、ｄθ／ｄｔ＝０であるので、移動体１０のｌ−θ軌道が、一度この直線を越えて図１３の領域１に入ると、２度と領域２に入ることは無い。

したがって、移動体１０において、式（２．１１）のフィードバック制御を行うと、直線θ＝βｌと、θ＝β・ｌ・α／（α＋βｖ_ｒｅｆ）に挟まれた領域内でｌ、θは変化することになる。その結果、ｌとθは振動せず、同時に０に収束する。
したがって、演算手段１４１は、式（２．１１）に基づいて、移動体１０の旋回速度に関する演算を行い、制御手段１４０は、演算結果に基づいて適宜フィードバック制御を行う。

また、式（２．７）ではコントローラの操作により、移動体１０に与える速度を求めたが、この速度を車輪速度に変換する方法について説明する。
このとき、演算手段１４１は、車輪速度が過大とならないような車輪速度を求める。例えば、移動体１０が物理的に走行不可能な車輪速度を設定した場合、軌道に追従できなくなるためである。
以下に、車輪速度を抑制する方法について詳述する。

車輪速度は、式（２．３）の逆行列Ｊ_ｒ ^−１に、式（２．７）で求めたｖ^＊を掛ければよい。すなわち、車輪速度を

とすると、

となるが、

になるように制限する。ここで、

は、例えば、移動体１０がハードウェアとして走行可能な最高速度とする。具体的には以下のように設定する。

ここで、式（３．２）の最高速度は、

に制限される。

図１４は、左右の車輪速度の制限を示す図である。式（３．２）が適用される場合には、図１４の円周上の車輪速度となり、式（３．３）が適用される場合には、円内の車輪速度をとることとなる。

実装は以下のように行う。

であるため、演算手段１４１は、まず、

を計算し、左右の車輪速度を求める。次に、

を計算し、許容する車輪速度

と比較する。すなわち、

とする。
これにより、移動体１０に入力されたフィードフォワード量が過大となっても、左右の車輪の比は保存され、軌道に追従することができる。

したがって、移動体１０は、速度を指令する際に、式（４．１）を利用することにより、軌道との並進方向についてはフィードフォワードで速度を設定し、旋回速度をフィードバック制御により設定することができる。

さらに、移動体１０の左右の車輪速度を抑制することができる。

なお、移動体１０は、最短距離方向と軌道の接線方向が直角であることが前提であり、例えば軌道の始点における接線と現在の移動体１０との関係において、角度偏差が直角から大きく外れている場合など、前提が成り立たない場合がある。
この場合には、現在の移動体１０の位置を始点として軌道を生成し直すことや、距離偏差が軌道に対して直角になる位置から処理をはじめること、別の方法により軌道の始点となる位置まで移動するなどの対策を、あらかじめ行うことが望ましい。
また、移動体１０は、走行状態のまま軌道の終点に到達すると、終点で停止することができず、さらにその後の挙動が不安定になる場合がある。この場合には、移動体１０は、終点付近では減速を行い、終点で停止するよう制御することが望ましい。

これにより、移動体１０は、軌道に乗るように位置を調節する際に軌道をオーバーシュートすることや振動が発生することを防止して、走行することができる。また、移動体１０は、軌道との並進方向についてはフィードフォワードで速度を設定しているため、速度の振動が発生するのを防止することができる。
さらに、移動体１０では、フィードフォワード値が過大である場合に駆動輪１３０の速度を抑制し、左右の車輪の動作比を保持することにより、ハードウェアの制限またはソフトウェアの制限によって駆動輪１３０が動作指示の通りに動作せず、軌道から外れることを防止することができる。
したがって、移動体１０は、設定された軌道に対し、適切に走行することができる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、距離偏差と角度偏差を求める演算手段と、駆動輪の回転速度と距離偏差および角度偏差の時間変化との関係を算出する演算手段は、別の演算手段であってもよい。

１０ 移動体
１１０ 位置情報取得手段
１２０ 記憶手段
１３０ 駆動輪
１４０ 制御手段
１４１ 演算手段
１４２ 設定手段

Claims (2)

1. 駆動輪を有する移動体であって、
   空間上における現在位置及び方向を取得する位置情報取得手段と、
   前記現在位置及び走行する軌道の情報を記憶する記憶手段と、
   制御手段と、を有し、
   前記制御手段は、前記現在位置と前記軌道との最短距離である距離偏差と、前記距離偏差を求めた位置における前記方向と前記軌道の方向の差である角度偏差を求めると共に、
   前記移動体の並進速度として、ユーザの入力により任意のフィードフォワード値を設定する設定手段と、を備え、
   前記制御手段は前記距離偏差と前記角度偏差と任意のフィードバック値を用いて前記距離偏差と前記角度偏差が収束するような旋回速度を算出し、
   フィードバックにより算出される値を用いずに、前記フィードフォワード値を用いて並進速度を算出し、前記並進速度と前記旋回速度に基づいて前記駆動輪の回転速度を決定する、
   移動体。
2. 前記演算手段は、前記並進速度と前記旋回速度を、前記駆動輪の回転速度に変換する演算を行い、
   前記制御手段は、前記変換された駆動輪の回転速度の２乗平均が、あらかじめ設定された最大速度を超える場合には、２乗平均と最大車輪速度の比に応じて前記駆動輪の回転速度を減補された状態で駆動し、あらかじめ設定された最大速度を超えない場合には、前記変換された前記駆動輪の回転速度で駆動するよう制御する、
   請求項１に記載の移動体。

Images