JP4577247B2 - 移動体及びその制御方法 - Google Patents

Description

本発明は、移動体及びその制御方法に関し、特に詳しくは目標経路に対して追従する移動体、及びその制御方法に関する。

従来より、ロボットや車両等の移動体を目標経路に対して自動追従させる技術が提案されている。例えば、目標経路に対する、移動体の位置の横ずれ量や方位角のずれ量をフィードバック制御することによって自動追従する技術がある。このような、自律型の移動体では、移動開始位置から目標位置までの目標経路を計画している。そして、移動体は、フィードバック制御されながら、目標経路に追従して移動する。また、このような移動体に対して、停止位置で所定の動作、作業を行なうロボットもある。

このような、自律型の移動体では、正確に目標経路に追従して移動させることが困難である。この問題点を解決するために、目標経路からの相対的な横ずれ及び方位角のずれ量に応じたフィードバック制御による操作量に加えて、目標経路の曲率や横ずれ量に応じたフィードフォワード制御による操作量により操舵角を決定する技術が提案されている（特許文献１参照）。この技術では、曲率が変化する任意の目標経路、目標経路に沿った座標軸を有する座標系に変換して、横ずれ量等を計算している。

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

しかしながら、上記の技術を用いても、移動体を正確に制御することは困難である。したがって、移動体の停止位置が目標位置からずれてしまうという問題点が生じてしまう。上記の文献では、目標経路に沿った座標軸において所定の座標まで移動すると、移動体が停止してしまう。したがって、目標位置近傍で横ずれ量が大きくなった場合、停止位置が目標位置から大きくずれてしまう。停止位置で所定の動作、作業を行なう場合、停止位置のずれが生じると、所定の動作、作業を行なうことができなくなってしまう。例えば、物体を把持するハンドなどを有する移動体の場合、停止位置が目標位置からずれてしまうと、物体を把持することができないという問題点が生じる。このように、従来の移動体では、停止位置を正確に制御することが困難であるという問題点が生じる。

本発明は、かかる課題を解決するためになされたものであり、停止位置の制御を容易に行うことができる移動体、及びその制御方法を提供することを目的とする。

本発明の第１の態様にかかる移動体は、目標位置までの目標経路に対して追従するよう移動制御を行う制御部を有する移動体であって、前記制御部が、前記目標経路に追従して移動する移動体の位置を推定する位置推定部と、前記目標位置を含む、半径がしきい値ｋに応じた大きさを有する目標領域であって、前記目標位置を通る分割線によって第１の分割領域と第２の分割領域とに分割されており、前記分割線を前記目標位置における前記目標経路の方向と直交する方向の線分として、前記第１の分割領域と前記第２の分割領域とを分割している目標領域を設定する領域設定部とを備え、前記位置推定部で推定された推定位置が第１の分割領域から第２の分割領域に遷移した時点で、前記移動体が目標位置に到達したと判断して、前記移動体の移動を停止するものである。これにより、停止位置の制御を容易に行うことができる。

本発明の第２の態様にかかる移動体は、上記の移動体において、前記分割線を前記目標位置における前記目標経路の方向と直交する方向の線分として、前記第１の分割領域と前記第２の分割領域とを分割するものである。これにより、停止位置をより正確に制御することができる。

本発明の第３の態様にかかる移動体は、上記の移動体において、前記目標領域が、半径ｋの円形として設定されているものである。

本発明の第４の態様にかかる移動体は、上記の移動体において、前記移動体が、前記目標位置までの目標経路を計画する経路計画部と、前記位置推定部で推定された推定位置が、前記経路計画部で計画された目標経路上の位置から所定距離だけ離れたことを判定する判定部とを、さらに備え、前記経路計画部が、前記目標経路から前記移動体が前記目標領域の半径ｋ以上の距離を離れたと判定された場合に、前記目標位置までの目標経路を再計画することを特徴とするものである。これにより、目標経路に対して追従させることができる。

本発明の第５の態様にかかる移動体は、上記の移動体において、前記領域設定部が、前記目標経路及び前記半径ｋの値に基づいて、移動可能領域を設定し、前記判定部が、前記推定位置が前記移動可能領域から外れたと判定した場合に、前記目標経路を再計画させるものである。これにより、複雑な処理を行うことなく、容易に判定することができる。

本発明の第６の態様にかかる移動体は、上記の移動体において、所定のタスクを実行するタスク処理機構をさらに備え、前記タスク処理機構が前記移動体の停止位置からタスクを実行するために必要となる距離で設定されるタスク可能範囲に基づいて前記半径ｋの値が設定されているものである。これにより、停止位置でのタスクを確実に実行することができる。

本発明の第７の態様にかかる移動体の制御方法は、目標経路に対して追従する移動体の移動を制御する制御方法であって、前記目標位置を含む、半径がしきい値ｋに応じた大きさを有する目標領域であって、前記目標位置を通る分割線によって第１の分割領域と第２の分割領域に分割されており、前記分割線を前記目標位置における前記目標経路の方向と直交する方向の線分として、前記第１の分割領域と前記第２の分割領域とを分割する目標領域を設定するステップと、前記移動体の位置を推定するステップと、前記推定位置が前記分割領域のうちの第１の分割領域から第２の分割領域に遷移した時点で、目標位置に到達したと判断して、前記移動体の移動を停止するステップと、を有するものである。これにより、停止位置の制御を容易に行うことができる。

本発明の第８の態様にかかる移動体の制御方法は、上記の制御方法であって、前記分割線を前記目標位置における前記目標経路の方向と直交する方向の線分として、前記第１の分割領域と前記第２の分割領域とを分割するものである。これにより、停止位置をより正確に制御することができる。

本発明の第９の態様にかかる移動体の制御方法は、上記の制御方法であって、前記目標領域が、半径ｋの円形として設定されているものである。

本発明の第１０の態様にかかる移動体の制御方法は、上記の制御方法であって、前記移動体が、前記目標位置までの目標経路を計画するステップと、前記位置推定部で推定された推定位置が、前記経路計画部で計画された目標経路上の位置から所定距離だけ離れたことを判定するステップとを、さらに備え、前記目標経路から前記移動体が前記目標領域の半径ｋ以上の距離を離れたと判定された場合に、前記目標位置までの目標経路を再計画することを特徴とするものである。これにより、目標経路に対して追従させることができる。

本発明の第１１の態様にかかる移動体の制御方法は、上記の制御方法であって、前記目標経路及び前記しきい値に基づいて、移動可能領域を設定するステップをさらに備え、前記判定するステップでは、前記推定位置が前記移動可能領域から外れたと判定した場合に、前記目標経路を再計画させるものである。これにより、複雑な処理を行うことなく、容易に判定することができる。

本発明の第１２の態様にかかる移動体の制御方法は、上記の制御方法であって、前記位置移動を停止した後、前記移動体を目標姿勢までその場旋回させるステップをさらに備えるものである。これにより、停止位置姿勢を容易に制御することができる。

本発明によれば、停止位置を容易に制御することができる移動体、及びその制御方法を提供することができる。

発明の実施の形態１．
本実施の形態にかかる移動体について図１を用いて説明する。図１（ａ）は、本実施の形態にかかる移動体の構成を模式的に示す上面図であり、図１（ｂ）は本実施の形態にかかる移動体の構成を模式的に示す側面図である。１０は移動体、１１は車体、１２は駆動輪、１３は従動輪、１４は駆動機構、１５は制御部である。本実施の形態では、移動体１０を対向２輪型の車輪移動ロボットとして説明する。

移動体１０は、自律型の移動ロボットである。車体１１の側面下方には、駆動輪１２が設けられている。駆動輪１２は、車体１１の対向する側面にそれぞれ設けられている。車体１１の角近傍には、キャスタ等の従動輪１３が設けられている。２つの駆動輪１２は、モータ等を有する駆動機構１４によって、独立して回転する。すなわち、駆動機構１４が左右の駆動輪１２を回転させることによって、移動体１０が移動する。２つの駆動輪１２は、それぞれ異なる駆動機構１４に接続されている。したがって、駆動輪１２を異なる回転方向、回転速度で回転させることにより、移動体１０の移動方向、移動速度を制御することができる。具体的には、駆動輪１２を同じ方向、異なる速度で回転させることにより、移動体１０の移動方向が変化しながら、移動する。すなわち、２つの駆動輪１２の距離、及び回転速度の差に応じて、移動体１０がカーブしながら移動する。また、駆動輪１２を反対方向に回転させることによって、移動体１０がその場で旋回する。さらに、駆動輪１２を同じ方向、同じ速度で回転させることにより、移動体１０が直進移動する。

制御部１５は、移動体１０が目標経路に追従するよう、駆動機構１４を制御する。例えば、制御部１５は、移動体１０の現在位置、及び目標位置に基づいて目標経路を計画する。そして、制御部１５は、移動体１０が目標経路に追従して移動するよう、駆動機構１４をフィードバック制御、あるいはフィードフォワード制御を行う。制御部１５は、例えば、駆動機構１４のモータ又は駆動輪１２に取り付けたロータリーエンコーダや、車体１１に取り付けたジャイロセンサ等からの測定値に基づいて現在の位置を推定する。そして、制御部１５は、推定位置に基づいてフィードバック制御を行う。これにより、駆動機構１４のモータが適切な回転角度で回転する。そして、駆動輪１２が回転して、移動体１０が目標経路に追従して移動する。

本発明の実施の形態にかかる移動体の制御部１５は、例えば、車体１１に搭載された演算処理装置により実現される。この演算処理装置は、例えば、中央処理装置（ＣＰＵ）、ＲＯＭ、ＲＡＭ、ハードディスク等の記憶装置、入出力インターフェース等を備えている。ＲＯＭ、等の記憶装置には、オペレーティングシステムと協働してＣＰＵ等に命令を与え、アプリケーションプログラムを記録することができ、ＲＡＭにロードされることによって実行される。このアプリケーションプログラムは、本発明にかかる制御方法を実現する特有のプログラムを含む。例えば、目標経路を計画するプログラム、センサからの測定値に基づいて位置を推定するプログラムなどを有している。制御部１５による追従制御は、中央処理装置がアプリケーションプログラムをＲＡＭ上に展開した上で当該アプリケーションプログラムに従った処理を記憶装置に格納されたデータを読み出し、また格納を行なうことにより、実行される。

制御部１５による移動制御について説明する。図２は、目標位置姿勢までの目標経路を模式的に示す図である。ここで、移動体１０の位置姿勢をＲ（ｒｘ，ｒｙ，ｒｔ）とする。すなわち、２次元直交座標系における移動体１０の位置がｒｘ，ｒｙで示され、移動体１０の姿勢（方向）がｒｔで示される。なお、ここでは、駆動輪１２の中心を移動体１０の位置とする。また、制御部１５には、目標位置姿勢Ｔ（ｔｘ，ｔｙ，ｔｔ）が設定されている。制御部１５は目標位置までの目標経路Ｐを計画する。制御部１５は、目標位置姿勢Ｔ（ｔｘ，ｔｙ，ｔｔ）と移動体の初期の位置姿勢Ｒ（ｒｘ，ｒｙ，ｒｔ）から目標経路Ｐを計画する。制御部１５は、推定された推定位置と目標経路Ｐとに基づいて、フィードバック制御等を行う。これにより、移動体１０が目標経路Ｐに沿って移動するよう制御される。また、目標位置姿勢Ｔを原点、ｔの向きをｘ軸としたローカル座標系をＬ（ｌｘ，ｌｙ）とする。このとき、移動体１０のローカル座標はＲｌ（ｌｒｘ，ｌｒｙ）で示される。

ここで、制御部１５は計画された目標経路Ｐに基づいて移動許容領域を設定する。移動体１０が移動許容領域の外側になると、制御部１５は、目標経路を再計画する。すなわち、推定された位置が移動許容領域の外側になった時点で、制御部１５は、目標経路の再計画を開始する。このとき、制御部１５は推定された現在の位置姿勢と、目標位置姿勢Ｔとに基づいて目標経路を再計画する。そして、制御部１５は、移動体１０が再計画された目標経路に追従するよう駆動機構１４を制御する。

ここで、移動体１０が目標経路Ｐからｋ以上離れないよう、移動許容領域が設定されている。この移動許容領域から移動体１０が外れたとき、目標経路Ｐが再計画される。移動体１０の目標経路Ｐまでの距離がしきい値ｋを超えた場合に、制御部１５は、再計画を実行する。従って、目標経路Ｐからｋの距離にある許容ラインＱ１、Ｑ２で挟まれた領域が移動許容領域となる。換言すると、半径ｋの円の中心を目標経路Ｐ上に沿って移動させたときの円の軌跡が移動許容領域となる。移動体１０が、目標経路Ｐと垂直な方向（左右方向）にｋ以上離れた場合に、目標経路が再計画される。すなわち、目標経路Ｐの最寄点までの距離がｋ以上となった場合、現在の位置姿勢Ｒを始点として目標経路を再計画する。しきい値ｋは例えば、約１０ｃｍである。

さらに、制御部１５は、目標位置（ｔｘ，ｔｙ）を中心とする半径ｋの円を設定する。この中心（ｔｘ，ｔｙ）、半径ｋの円を設定領域Ｃとする。この設定領域Ｃは目標位置を含むも目標領域となる。そして、制御部１５は、目標位置（ｔｘ，ｔｙ）を通る分割線Ｄで設定領域Ｃを２つに分割する。これにより、半円状の分割領域Ａ、及び分割領域Ｂが生成される。分割線Ｄは、目標姿勢ｔｔに直交し、しきい値ｋに基づく長さを有する線分である。すなわち、分割領域Ａ、及び分割領域Ｂは、目標姿勢ｔｔに直交する方向の線分を介して隣接する。設定領域Ｃを分割領域Ａ、及び分割領域Ｂに分割する分割線Ｄは、目標位置を通り、２ｋの長さを有する線分である。

ここで、制御部１５は以下の３つの停止条件のうち、いずれか一つの条件を満たしたとき、移動体１０の位置移動を停止させる。
（１）分割領域Ａを通過して、分割領域Ｂに入ったとき
（２）分割領域Ｂを通過して、分割領域Ａに入ったとき
（３）ｌｘ＝０、ｌｙ＝０のとき

そして、その後、その場で旋回して、ｒｘ＝ｔｔとなった時点で停止する。これにより、一つの変数ｋを適切に決めるだけで、停止時の移動体１０の位置姿勢Ｒは以下を満たすことが保証される。
（ａ）ｒｔ＝ｔｔ、すなわち、姿勢は、目標姿勢と一致する。
（ｂ）ｌｒｘ＝０、−ｋ＜ｌｒｙ＜ｋ、すなわち、移動体１０はｔｔの向きと直交する方向の線上で停止する。そして、ｌｒｙが±ｋの範囲内となる。このように、本実施の形態にかかる移動体１０では、停止位置を容易に制御することができる。

制御部１５は、推定位置に基づいて、（１）、及び（２）の停止条件を満たすか否かを判定する。上記のように、（１）、及び（２）の停止条件を設定することにより、移動体１０の停止位置は（ｂ）を満たす。さらに、（３）の停止条件を設定することにより、移動体１０が、分割線Ｄに沿って移動する場合であっても、目標位置で移動体１０が停止する。このように、（１）〜（３）のタイミングに基づいて移動体１０の位置移動を停止することで、停止位置のずれを防ぐことができる。

このように制御することによって、停止位置を容易に制御することができる。従来、高い停止位置精度で目標位置まで移動させようとした場合、移動体がその場で回転してしまうことがあった。しかしながら、本発明を利用することにより、容易に停止位置の誤差を低減することができる。このように、推定位置が、分割領域Ａから分割領域Ｂに遷移した時点、又は分割領域Ｂから分割領域Ａに遷移した時点で、移動を停止する。すなわち、移動体１０が分割線を横切った時点で、移動体１０が目標領域に到達したと判断して、移動を停止する。

次に、制御部１５の構成について図３を用いて説明する。図３は、本実施の形態かかる移動体１０に用いられる制御部１５の構成を示すブロック図である。制御部１５には、駆動制御部２１と、経路計画部２２と、領域設定部２３と、位置推定部２４と、判定部２５と、停止位置制御部２６とが設けられている。

駆動制御部２１は、駆動輪１２を駆動するための駆動機構１４を制御する。具体的には、駆動機構１４に設けられたモータの回転数、回転方向の制御を行う。すなわち、駆動制御部２１は、左右の駆動機構１４にそれぞれ駆動信号を出力する。さらに、駆動制御部２１には、駆動機構１４に設けられたセンサからの測定信号が入力される。センサからの測定信号は、駆動制御部２１を介して、位置推定部２４に入力される。位置推定部２４は、センサからの測定信号に基づいて、移動体１０の現在位置を推定する。さらに、駆動制御部２１は、経路計画部２２で計画された目標経路と、位置推定部２４で推定された現在位置に基づいてフィードバック制御を行う。なお、経路計画部２２による目標経路の計画、及び位置推定部２４による位置の推定については後述する。さらには、駆動制御部２１は、フィードフォワード制御、に加えてフィードフォワード制御を実行する。

目標位置姿勢Ｔが設定されると、経路計画部２２は、移動体１０の初期位置姿勢と目標位置姿勢Ｔとに基づいて、目標経路Ｐを設定する。例えば、移動体１０の目標軌道となる目標経路は、しばしば座標の集合として与えられる。座標は、例えば、グリッド状に離散化されたグリッド座標で表現される。グリッド座標を利用するメリットは、最短経路探索を容易に実行できる点や、確実に目的地に到達できる経路を得ることができる点にある。しかしながら、グリッド座標において表現される目標経路は直線と直線が接続された不連続な経路であるため、移動体１０がこのような経路を追従することは難しい。そのため、与えられた離散的な経路座標から、移動体１０の追従制御に利用可能な滑らかな目標経路を、例えば、重み付き移動平均を利用して補間曲線を求めることにより作成することができる。このように、与えられたマップに対してグリッドを形成することによって、容易に目標経路Ｐを計画することができる。もちろん、上記以外の方法によって目標経路Ｐを計画してもよい。

位置推定部２４は、移動体１０の現在の位置、及び姿勢を推定する。例えば、位置推定部２６は、駆動機構１４に設けられたロータリーエンコーダや、ジャイロセンサなどのセンサからの出力に基づいて現在の位置を推定する。例えば、現在位置の推定には、内界センサであるロータリーエンコーダを用いることができる。ロータリーエンコーダにより左右のモータ、あるいは駆動輪１２の回転数を測定する。そして、制御部１５は、測定された回転数に基づいて駆動輪１２の移動量を算出する。初期の位置姿勢からの移動軌跡を求めることによって、現在の推定位置を算出することができる。また、姿勢の推定については、ジャイロセンサなどを利用することが可能である。なお、位置推定については、上記の方法に限られるものではない。例えば、加速度センサ、レーザレンジファインダ、カメラなどのセンサを用いたものを使用する。もちろん、複数の種類のセンサを用いて、位置を推定してもよい。

領域設定部２３は、経路計画部２２で計画された目標経路Ｐに基づいて移動許容領域、及び設定領域Ｃを設定する。さらに領域設定部２３は、設定領域Ｃを分割して分割領域Ａ、及び分割領域Ｂを設定する。すなわち、半径ｋの設定領域Ｃを分割線Ｄで仕切って、分割領域Ａ、及ぶ分割領域Ｂを生成する。したがって、分割領域Ａ、及び分割領域Ｂは、分割線Ｄで仕切られている。すなわち、分割線Ｄは、分割領域Ａと分割領域Ｂとの境界線となる。領域設定部２３は、与えられたマップに対して、移動許容領域、設定領域Ｃ、及び分割領域Ａ，Ｂを設定する。領域設定部２３は、目標位置を含む半径ｋの円形の領域を設定領域Ｃとして設定する。ここで、設定領域Ｃの中心は目標位置となる。ここで、設定領域Ｃは移動許容領域に含まれ、分割領域Ａ、Ｂは設定領域Ｃに含まれる。領域設定部２３は、これらの領域をＲＡＭなどの記憶装置に記憶させる。このように、目標経路、及び目標位置姿勢を参照して、それぞれの領域が設定される。

判定部２５は、移動体１０が移動許容領域から外れているか否かを判定する。これにより、再計画を行なうか否かが判断される。具体的には、位置推定部２４で推定された位置が、領域設定部２３で設定された移動許容領域に含まれているか否かを判断する。そして、推定位置が移動許容領域に含まれていないと判断されたとき、判定部２５は、経路計画部２２に再計画を実行させる。一方、推定位置が移動許容領域に含まれていると判断した場合、移動体１０の移動をそのまま続行する。なお、判定部２５は、移動許容領域に基づいて再計画の判定を行なうものに限られるものではない。例えば、判定部２５が現在の推定位置と、目標経路の最寄点との距離を算出して、その距離としきい値ｋとを比較してもよい。なお、移動許容領域は、目標経路からしきい値ｋだけ離れた許容ラインＱ１、Ｑ２に基づいて設定されれている。この移動許容領域に基づいて判定することにより、目標経路の最寄点までの距離を算出する必要がなくなるため、演算処理を簡素化することができる。

このように、判定部２５が、移動許容領域の外側にあると判定した場合、経路計画部２２が目標経路を再計画する。さらに、経路計画部２２は、領域設定部２３に再計画された目標経路に対する領域を再設定させる。領域設定部２３は、再計画された目標経路に対して移動許容領域を設定する。すなわち、目標経路Ｐが変化したため、領域設定部２３は移動許容経路を再設定する。ここで、目標位置姿勢Ｔについては変化がないため、分割領域Ａ、分割領域Ｂ、及び設定領域Ｃは変わらない。すなわち、再計画前後で、分割領域Ａ、分割領域Ｂ、及び設定領域Ｃは変化しない。そして、判定部２５は再設定された移動許容領域に基づいて、再計画を行なうか否かの判定を行なう。そして、再設定された移動許容領域から移動体１０がはみ出した場合は、再度、同様の処理を行う。

停止位置制御部２６は、移動体１０の停止位置を制御する。具体的には、停止位置制御部２６は、上記の（１）〜（３）の条件を満たすか否かを判定する。そして、停止位置制御部２６が（１）〜（３）のいずれかの条件を満たしたと判定した場合、位置移動を停止する。これにより、移動体１０の停止位置が決定される。すなわち、位置推定部２４で推定された位置が、分割領域Ａ、又は分割領域Ｂに含まれている否かを判定する。そして、次の推定位置が他方の分割領域に含まれている否かを判定する。さらに、推定位置が停止位置と一致していないかを判定する。そして、上記の（１）〜（３）の条件のいずれかを満たした場合、位置移動を停止する。このように停止位置制御部２６は、一方の分割領域から他方の分割領域に遷移した時点で、目標位置に到達したと判断する。そして、移動を停止する。

例えば、推定位置が分割領域Ａに入っていた場合、停止位置制御部２６は、次の推定位置が分割領域Ｂに含まれている否かを判定する。そして、次の推定位置が、分割領域Ｂに入っていない場合、駆動制御部２１に移動体１０の移動を続行させる。一方、次の推定位置が分割領域Ｂに入った場合、移動体１０が分割領域Ａから分割領域Ｂに入ったと判断する。すなわち、移動体１０が分割線Ｄを横切ったと判断する。そして、停止位置制御部２６は位置移動を停止させる。具体的には、停止位置制御部２６は、駆動制御部２１に位置移動を停止するよう命令する。これにより、駆動制御部２１は駆動機構１４を制御して、駆動輪１２の回転を停止する。その後、駆動制御部２１は、左右の駆動輪１２を反対方向に回転させて、その場で旋回させる。これにより、移動体１０が目標姿勢ｔｔとなる。もちろん、移動体１０が分割領域Ｂから分割領域Ａに入った場合も、同様に位置移動を停止する。

このように、時間的に連続して推定された推定位置のうち、一方が分割領域Ａに含まれ、他方が分割領域Ｂに含まれたタイミングに基づいて、移動体１０の位置移動を停止する。これにより、分割線Ｄを横切り、第１の分割領域（例えば、分割領域Ａ）から第２の分割領域（例えば、分割領域Ｂ）に入ったタイミングに基づいて位置移動が停止する。そして、位置移動が停止したら、移動体１０をその場で目標姿勢まで旋回させる。このようにして、移動体１０の目標位置姿勢Ｔまでの移動を制御することができる。これにより、停止位置の誤差を低減することができる。具体的には、目標姿勢と直交する方向における停止位置の誤差を±ｋに抑えることができる。

制御部１５には、例えば、位置推定プログラム、経路計画プログラム、領域設定プログラム、再計画判定プログラム、及び停止位置制御プログラム等が記憶されている。そして、これらのプログラムを実行することにより、上記の処理が行われる。

次に、図４を用いて本実施の形態にかかる制御方法について説明する。図４は、本実施の形態にかかる移動体１０の制御方法を示すフローチャートである。

まず、経路計画部２２が設定された目標位置姿勢Ｔと、移動体１０の初期位置姿勢に基づいて経路を計画する（ステップＳ１）。これにより、目標経路が設定される。そして、領域設定部２３がこの目標経路Ｐに基づいて、領域を設定する（ステップＳ２）。ここでは、移動許容領域、分割領域Ａ，Ｂ、及び設定領域Ｃが設定される。次に、駆動制御部２１が駆動機構１４を駆動して、移動体１０の移動を開始する（ステップＳ３）。これにより、移動体１０が目標経路に追従して移動する。

移動が開始したら、位置推定部２４は、移動体１０の現在の位置を推定する（ステップＳ４）。判定部２５は、最新の推定位置に基づいて再計画を行なうか否かの判定を行なう（ステップＳ５）。すなわち、判定部２５は、推定位置がステップＳ２で設定された移動許容領域から外れているか否かを確認する。そして、移動許容領域から外れていた場合、経路を再計画する（ステップＳ６）。これにより、現在の推定位置姿勢と目標位置姿勢とに基づいて目標経路Ｐが再計画される。ここでは、例えば、移動体１０の移動を停止して、経路が再計画される。再計画が終了したら、移動体１０の移動開始ステップＳ３に戻り、処理を繰り返す。もちろん、移動体１０を移動したまま、経路を再計画してもよい。

推定位置が移動許容領域から外れてない場合、停止位置制御部２６が位置停止判定を行なう（ステップＳ７）。具体的には、上記の通り、（１）〜（３）の条件を満たすか否かを判定する。（１）〜（３）の条件を満たさない場合は、現在の位置を推定するステップ（Ｓ４）に戻り、処理を繰り返す。すなわち、現在位置の推定を再度行なう。したがって、位置推定部２４は、所定の時間間隔で、移動体１０の現在の位置を更新していく。ここで、位置推定部２４によって推定された位置を、メモリ等に蓄積していってもよい。

（１）〜（３）のいずれかの条件を満たした場合、位置移動を停止する（ステップＳ８）。すなわち、左右の駆動輪１２の同方向の回転が停止し、停止位置が決定する。これにより、移動体１０が目標位置、あるいはその近傍で停止する。そして、左右の駆動輪１２を反対方向に回転させ、回転動作を行なう（ステップＳ９）。これにより、移動体１０が目標姿勢となる。そして、駆動機構１４の駆動を停止して、目標位置姿勢までの移動を終了する。これにより、停止位置の誤差を小さくすることができる。停止姿勢を目標姿勢と一致させることができる。

なお、上記の例では、分割線Ｄを円形の設定領域Ｃを２等分する線分として説明したが、これに限るものではない。分割線Ｄは、目標位置を通る線であればよい。したがって、分割線Ｄは、直線に限らず、曲線であってもよい。また、分割線Ｄの長さは、２ｋに限らず、ｋに応じた長さであればよい。また、設定領域Ｃも半径ｋの円形に限るものではない。すなわち、設定領域Ｃは、目標位置を含み、しきい値ｋに応じた大きさを有する領域であればよい。また、分割領域の数は２に限るものではない。

発明の実施の形態２．
本実施の形態にかかる移動体１０について図５を用いて説明する。図５は、本実施の形態にかかる移動体１０の構成を模式的に示す上面図である。本実施の形態にかかる移動体１０の移動制御は、実施の形態１と基本的に同様である。したがって、実施の形態１と同様の内容については、説明を省略する。本実施の形態にかかる移動体１０は、物体を把持するタスクを行なうロボットである。

移動体１０の車体１１には、物体４０を把持するためのハンド３３が取り付けられている。このハンド３３を閉じることにより、対象となる物体４０が把持される。また、車体１１には回転機構３１を介してアーム３２が取り付けられている。また、アーム３２は直動アクチュエータを有しており、伸縮可能に設けられている。本実施の形態では、アーム３２の伸縮範囲を０〜１００ｍｍとする。そして、アーム３２の先端にハンド３３が設けられている。

回転機構３１は、アーム３２を所定の角度だけ回転させることができる。ここで、回転機構３１の回転範囲を±３０°とする。移動体１０は、目標位置姿勢まで移動したら、ハンド３３で物体４０を把持する。このとき、回転機構３１、及びアーム３２を駆動する。これにより、物体４０を把持可能な位置にハンド３３が移動する。例えば、ＣＣＤカメラなどのカメラで物体４０の位置を認識して、把持可能な位置にハンド３３を移動させることができる。このように、アーム３２、及び回転機構３１を設けることによって、移動体１０の停止位置に生じた誤差を吸収することができる。したがって、停止位置にずれが生じた場合でも、物体４０を把持することができる。ここで、ハンド３３が把持することができる範囲は把持可能範囲Ｇとなる。

移動体１０の停止位置は上述の通り、目標姿勢と垂直な方向に±ｋの誤差がある。ここで、移動体１０の停止位置の範囲を停止位置範囲Ｅで示す。さらに、アーム３２を０ｍｍ、回転機構３１を０°にした時に、停止位置誤差に起因して生じるハンドの位置誤差をハンド位置範囲Ｆで示す。停止位置範囲Ｅとハンド位置範囲Ｆは、同じ大きさとなる。

ここで、回転機構３１と、直動のアーム３２とを組み合わせた場合、アーム３２の可動範囲は限られている。したがって、ハンド３３の把持可能範囲Ｇも限られる。移動体１０の停止位置がずれてしまうと、物体４０を把持することができる。換言すると、移動体１０の停止位置のずれをある許容範囲以下にしないと、物体４０を把持することができない。さらに、アーム３２の可動範囲は、方向性を持っている。そのため、移動体１０に許容される停止位置のばらつきも、進行方向に短く、進行方向と直交方向に長くなる。すなわち、目標姿勢と同じ方向については、停止位置の許容誤差が厳しくなる。一方、目標姿勢と直交する方向については、停止位置の許容誤差がある程度認められる。

ここで、しきい値ｋを把持可能範囲Ｇに基づいて設定する。そして、実施の形態１で示した方法を用いて、移動体１０を移動させる。これにより、進行方向の位置誤差、及び向きの誤差が無くなり、進行方向と直交方向の誤差を±ｋに抑えることができる。これにより、ハンド３３の把持可能範囲Ｇに把持の対象となる物体４０を治めることができる。したがって、物体４０を確実に把持することができる。さらに、把持可能範囲Ｇに応じて分割線Ｄを設定することにより、物体４０を確実に把持することができる。例えば、分割線Ｄを円弧状とすることも可能である。

また、本実施の形態では、移動体１０を把持可能なハンドを有するロボットとして説明したが、これに限るものではない。本発明は、停止位置で所定のタスクを行うタスク実行機構が設けられた移動体１０に対して適用可能である。この場合、タスク実行機構のタスク可能範囲に応じてしきい値ｋ、及び分割線Ｄを設定すればよい。ここでタスク可能範囲は、移動体の停止位置からタスクを実行するために必要となる距離で設定することができる。例えば、移動体が物体を把持するハンド部を有するロボット型移動体の場合、ロボットの把持対象の物体から、ハンドが届く最大の距離によって特定される領域をタスク可能領域として設定する。これによって、移動体１０の位置が、移動体１０によって実行されるタスクに影響がない範囲に含まれている場合であれば、移動体１０が目標位置に到達したとみなして停止する。そして、この停止位置でその場旋回することにより、所定のタスクを確実に実行させることができる。

その他の実施の形態．
実施の形態１、２では移動体１０を２輪走行の移動体に適用したが、これに限らず、２輪よりも多い、例えば４輪走行のロボットにも適用でき、さらには、１輪走行のロボットや２足歩行・４足歩行のロボットにも適用できる。歩行ロボットの場合には歩幅を制御することにより移動方向を変えて目標経路を追従することができる。さらには、ロボットに限らず、２輪走行や４輪走行の車両であっても適用できる。

本発明の実施の形態１にかかる移動体の構成を示す図である。 本発明の実施の形態１にかかる移動体の制御方法を説明するための図である。 本発明の実施の形態１にかかる移動体の制御部の構成を示すブロック図である。 本発明の実施の形態１にかかる移動体の制御方法を示すフローチャートである。 本発明の実施の形態２にかかる移動体の構成を示す図である。

符号の説明

１０ 移動体、１１ 車両、１２ 駆動輪、１３ 従動輪、１４ 駆動機構、
１５ 制御部、２１ 駆動制御部、２２ 経路計画部、２３ 領域設定部、
２４ 位置推定部、２５ 判定部、２６ 停止位置制御部、

Claims (8)

1. 目標位置までの目標経路に対して追従するよう移動制御を行う制御部を有する移動体であって、
   前記制御部が、
   前記目標経路に追従して移動する移動体の位置を推定する位置推定部と、
   前記目標位置を含む、半径がしきい値ｋに応じた大きさを有する目標領域であって、前記目標位置を通る分割線によって第１の分割領域と第２の分割領域とに分割されており、前記分割線を前記目標位置における前記目標経路の方向と直交する方向の線分として、前記第１の分割領域と前記第２の分割領域とを分割している目標領域を設定する領域設定部とを備え、
   前記位置推定部で推定された推定位置が第１の分割領域から第２の分割領域に遷移した時点で、前記移動体が目標位置に到達したと判断して、前記移動体の移動を停止する移動体。
2. 前記移動体が、前記目標位置までの目標経路を計画する経路計画部と、
   前記位置推定部で推定された推定位置が、前記経路計画部で計画された目標経路上の位置から所定距離だけ離れたことを判定する判定部とを、さらに備え、
   前記経路計画部が、前記目標経路から前記移動体が前記目標領域の半径ｋ以上の距離を離れたと判定された場合に、前記目標位置までの目標経路を再計画することを特徴とする請求項１に記載の移動体。
3. 前記領域設定部が、前記目標経路及び前記半径ｋの値に基づいて、移動可能領域を設定し、
   前記判定部が、前記推定位置が前記移動可能領域から外れたと判定した場合に、前記目標経路を再計画させる請求項２に記載の移動体。
4. 所定のタスクを実行するタスク処理機構をさらに備え、
   前記タスク処理機構が前記移動体の停止位置からタスクを実行するために必要となる距離で設定されるタスク可能範囲に基づいて前記半径ｋの値が設定されている請求項１乃至３のいずれかに記載の移動体。
5. 目標経路に対して追従する移動体の移動を制御する制御方法であって、
   前記目標位置を含む、半径がしきい値ｋに応じた大きさを有する目標領域であって、前記目標位置を通る分割線によって第１の分割領域と第２の分割領域に分割されており、前記分割線を前記目標位置における前記目標経路の方向と直交する方向の線分として、前記第１の分割領域と前記第２の分割領域とを分割する目標領域を設定するステップと、
   前記移動体の位置を推定するステップと、
   前記推定位置が前記分割領域のうちの第１の分割領域から第２の分割領域に遷移した時点で、目標位置に到達したと判断して、前記移動体の移動を停止するステップと、を有する制御方法。
6. 前記移動体が、前記目標位置までの目標経路を計画するステップと、
   前記位置推定部で推定された推定位置が、前記経路計画部で計画された目標経路上の位置から所定距離だけ離れたことを判定するステップとを、さらに備え、
   前記目標経路から前記移動体が前記目標領域の半径ｋ以上の距離を離れたと判定された場合に、前記目標位置までの目標経路を再計画することを特徴とする請求項５に記載の移動体の制御方法。
7. 前記目標経路及び前記半径ｋの値に基づいて、移動可能領域を設定するステップをさらに備え、
   前記判定するステップで、前記推定位置が前記移動可能領域から外れたと判定した場合に、前記目標経路を再計画させる請求項６に記載の移動体の制御方法。
8. 前記位置移動を停止した後、前記移動体をその場で目標姿勢まで旋回させるステップをさらに備える請求項５乃至７のいずれかに記載の移動体の制御方法。

Images