JP2013237125A - 移動体の移動制御方法 - Google Patents

Abstract

【課題】移動体が、確実に物体を把持すること。
【解決手段】移動体に設置された把持物体検出部が把持対象物体を検出することが可能な領域である物体検出可能領域と、移動体に設置されたロボットアームが把持対象物体に到達することが可能な領域である物体到達可能領域と、に基づいて、ロボットアームにより把持対象物体を把持可能な領域である物体把持可能領域を設定するステップと、移動体の自己位置ばらつきと、追従ずれと、に基づいて物体把持可能領域への到達可能性を判定するステップと、移動体を物体把持可能領域内に到達させる経路計画を生成するステップと、を備える。
【選択図】図１

Description

本発明は、移動体の移動制御方法に関する。

近年、自動制御によって目標走行経路を走行させ、目標とする位置に到達した後に、対象物体の把持等の作業を行う移動体の開発が進められている。

特許文献１には、目標位置を通る分割線をあらかじめ設定しておき、移動体は、自己位置推定により第１の分割領域から第２の分割領域に遷移した場合に、目標位置に到達したと判断して移動を停止する、移動体の制御方法が開示されている。

特許文献２には、無人搬送車の現在位置と操舵角により定まる目標停止線が、終点上にのるか、終点を超えたときに無人搬送車を停止させるよう制御する、移動体の制御方法が開示されている。

特許文献３には、ロボットアームの稼働作業空間内の領域を作業判定領域とするとともに、各作業判定領域内に格子点を配置するステップと、格子点において移動ロボットによる作業が遂行可能であるか否かを判断して、複数の作業に対応する作業可能領域を設定するステップとを有する移動体の制御方法が開示されている。

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

しかしながら、目的地を通る分割線をあらかじめ設定しておき、移動体が分割線に到達した場合に移動を停止させる場合であって、分割線と経路とが略並行となる経路が生成された場合には、移動体が分割線に到達せず移動体が停止しない可能性がある。また、自走する移動体がロボットアームを用いて作業することを仮定する場合に、自己位置推定結果がばらつく場合があり、経路追従の誤差と組み合わせると、目的地とのずれが大きくなる場合がある。すなわち、移動体が作業を行うことが可能な領域に、到達できない場合があった。

本発明にかかる移動体の移動制御方法は、移動体に設置された把持物体検出部が把持対象物体を検出することが可能な領域である物体検出可能領域と、前記移動体に設置されたロボットアームが前記把持対象物体に到達することが可能な領域である物体到達可能領域と、に基づいて、前記ロボットアームにより把持対象物体を把持可能な領域である物体把持可能領域を設定するステップと、前記移動体の自己位置ばらつきと、追従ずれと、に基づいて前記物体把持可能領域への到達可能性を判定するステップと、前記移動体を前記物体把持可能領域内に到達させる経路計画を生成するステップと、を備える。
これにより、自己位置ばらつきや追従ずれがある場合にも、移動体は物体把持可能領域に入ることが保証される。

移動体が、確実に物体を把持することができる。

実施の形態１にかかる移動体の構成を示した図である。 実施の形態１にかかる移動体の外観を示した図である。 実施の形態１にかかる移動体の動作のフローチャートである。 実施の形態１にかかる把持可能領域の計算の詳細なフローチャートである。 実施の形態１にかかる検出範囲を示す図である。 実施の形態１にかかる検出可能領域の算出を示す図である 実施の形態１にかかるロボットアームの手先点の集合を示す図である。 実施の形態１にかかる到達可能領域の算出を示す図である。 実施の形態１にかかる到達可能領域と検出可能領域を示す図である。 実施の形態１にかかる把持可能領域の算出を示す図である。 実施の形態１にかかる信頼楕円の算出を示す図である。 実施の形態１にかかる判定処理のフローチャートである。 実施の形態１にかかる移動体と信頼楕円との関係を示した図である。 実施の形態１にかかる把持対象物体から目的地を結ぶ直線と直線ｍの関係を示した図である。 実施の形態１にかかる把持可能範囲に収まりやすい経路生成のフローチャートである。 実施の形態１にかかる複数の経路点を設置した状態を示した図である。 実施の形態１にかかる複数の経路点を設置して経路を決定した状態を示した図である。 実施の形態１にかかる検出範囲に確実に入る経路生成を行うフローチャートである。 実施の形態１にかかる検出可能領域の端点を目的地に設定した状態を示した図である。 実施の形態１にかかる目的地から数点の経路点を配置した状態を示した図である。 実施の形態１にかかる目的地から数点の経路点を配置した後に経路を生成した状態を示した図である。 実施の形態１にかかる移動体が検出可能領域に入る直前の状態を示した図である。 実施の形態１にかかる移動体が検出可能領域に入るまで前進させた状態を示した図である。 実施の形態１にかかる既知である相対位置関係から移動体を移動させる状態を示した図である。

実施の形態１
以下、図面を参照して本発明の実施の形態について説明する。図１は、本発明に係る移動体の構成を示した図である。図２は、本発明に係る移動体の外観を示した図である。図２における破線の矢印は、移動体１の移動経路を示している。

移動体１は、ロボットアーム１１と、距離センサ１２と、環境地図データ記憶部１３と、自己位置推定部１４と、把持物体検出部１５、経路計画部１６と、駆動制御部１７と、演算部１８と、を備える。

ロボットアーム１１は、移動体１に設けられているアームである。典型的にはロボットアーム１１は、関節を有する腕部と、対象物体を把持する把持部を有する。ここでロボットアーム１１は、稼働範囲が狭く、指向性が高いものとする。すなわちロボットアーム１１は、自由度が低く、特定方向（前後・左右・斜め）にしか動かせないようなものとする。ロボットアーム１１の稼働範囲に関する情報は、経路計画部１６に出力される。

距離センサ１２は、移動体１から周囲の物体までの距離を測定するセンサである。例えば距離センサ１２は、超音波を発生させ、周囲の物体からの反射波に基づいて距離測定を行う超音波距離計である。距離センサ１２は、取得したデータを自己位置推定部１４に出力する。

環境地図データ記憶部１３は、環境地図データを記憶する。ここで環境地図データとは、移動体１が走行する環境内に設置された、種々の物体の位置情報が記憶されている。環境地図データ記憶部１３は、記憶された環境データを、自己位置推定部１４に出力する。

自己位置推定部１４は、距離センサ１２で取得した移動体１から周囲の物体までの距離のデータと、環境地図データ記憶部１３が記憶している環境地図データに基づいて、移動体１の自己位置を推定する。自己位置推定部１４は、推定した自己位置を経路計画部１６に出力する。

把持物体検出部１５は、カメラ等の撮像手段である。把持物体検出部１５は、検出範囲に関する情報を経路計画部１６に出力する。

経路計画部１６は、移動体１の移動経路を計画する。より具体的には、経路計画部１６は、自己位置推定部１４で推定した移動体１の自己位置と、ロボットアーム１１の稼働範囲等の情報に基づいて、移動体１がロボットアーム１１を用いて把持対象物体を把持可能な位置に移動するための経路を計画する。このとき経路計画部１６は、移動体１が把持対象物体に対して、正面に位置して物体を把持するタスクを仮定する。経路計画部１６は、計画した経路を、駆動制御部１７に出力する。

駆動制御部１７は、移動体１が経路計画部１６により計画された経路上を走行するよう、駆動部の動作を制御する。駆動部とは、例えば移動体１の下部に複数設けられている車輪を動作させるモータである。

演算部１８は、種々の演算を行う。例えば演算部１８は、把持物体検出部１５が把持対象物体を検出するために必要な、移動体１の移動先となる検出可能領域を演算により求める。また演算部１８は、ロボットアーム１１の稼働範囲等の情報に基づいて、ロボットアーム１１が把持対象物体に届くために必要な、移動体１の移動先となる到達可能点を算出する。さらに演算部１８は、検出可能領域及び到達可能点の情報に基づいて、把持対象物体を把持するために、移動体１の移動先となる把持可能領域の算出を行う。

次に、移動体１が自律移動を行い、把持対象物体を行う動作について説明する。図３は、移動体１の動作のフローチャートである。

把持物体検出部１５に、把持対象物体を設定する（ステップＳ１１）。例えば、把持物体検出部１５に把持対象物体が何であるかを登録する。

移動体１は、物体把持可能領域の計算を行う（ステップＳ１２）。図４は、物体把持可能領域の計算の詳細なフローチャートである。

演算部１８は、把持対象物体の検出可能領域を計算する（ステップＳ１２１）。図５Ａ及び図５Ｂは、把持物体検出部１５が有する検出範囲に基づいて、把持対象物体を検出可能な領域を幾何的に逆算して求めた図である。図５Ａの破線部は、把持物体検出部１５の検出範囲を示している。図５Ｂの破線部は、演算部１８の演算により求められた、把持物体検出部１５が把持対象物体を検出するために移動体１が到達すべき領域である検出可能領域を示している。

演算部１８は、物体到達可能領域の計算を行う（ステップＳ１２２）。具体的には演算部１８は、ロボットアーム１１の各関節の稼働範囲を離散点の集合として表し、全ての関節角の組み合わせにおいて手先座標ｒ＝ｆ（θ）を計算する。図６Ａは、演算部１８によるロボットアーム１１の手先座標の算出例であり、算出された手先座標を黒丸で示している。
さらに演算部１８は、把持対象物体の座標に基づいて手先座標の逆算を行うことにより、幾何的に移動体１の到達可能領域を算出する。すなわち演算部１８は、ロボットアーム１１の手先が把持対象物体に到達するための、移動体１の位置を演算する。図６Ｂの黒丸は、ロボットアーム１１の手先が把持対象物体に到達するために移動体１が到達すべき位置座標である到達可能点を示している。なお、到達可能点の集合した領域を到達可能領域とする。

演算部１８は、物体把持可能領域を計算する（ステップＳ１２３）。具体的には、ステップＳ１２１において演算部１８が算出した検出可能領域と、ステップＳ１２２において演算部１８が算出した到達可能点とを組み合わせ、検出可能領域にある到達可能点のみを残し、把持可能領域とする。図７Ａは、検出可能領域と到達可能点を組み合わせた状態を示している。図７Ｂは、検出可能領域と到達可能点との共通部である、把持可能領域を示している。

演算部１８は、把持可能領域を表す点の集合のｎ％信頼楕円を計算する（ステップＳ１２４）。ｎ＝１００とすると信頼楕円が無限大となるので、ｎは１００以外の値とし、例えばｎ＝９９とする。図８は、把持可能領域に対して９９％信頼楕円を生成した図である。この信頼楕円の内部を、把持可能領域とする。

移動体１は、目的地を信頼楕円の中心点に設定する（ステップＳ１３）。

経路計画部１６は、目的地に向かう経路を生成する（ステップＳ１４）。なお、経路計画部１６による経路の生成において、距離センサ１２を用いて測定した距離データと、環境地図データ記憶部１３に記憶された環境データに基づいて、自己位置推定部１４が自己位置推定を行う。

演算部１８は、自己位置ばらつき、追従ずれを考慮した到達判定を行う（ステップＳ１５）。図９は、詳細な判定処理のフローである。図１０は、移動体１と信頼楕円との関係を示した図である。

演算部１８は、生成された経路が目的地に入射する方向に対して垂直であり、目的地を通る直性ｍを設定する（ステップＳ１５１）。

演算部１８は、直線ｍと把持可能領域の信頼楕円の間の交点を求め、交点間の距離Ｌを算出する（ステップＳ１５２）。ここで、信頼楕円の長軸方向をｘ軸、単軸方向をｙ軸とする座標系を考え、ｍをｘ軸、ｙ軸の座標系に変換した場合、Ｌは（１）式のように求めることができる。

ここで、信頼楕円の長径を２ａ、短径を２ｂとする。

演算部１８は、移動体１の自己位置のばらつきの範囲の半径をΔｐと追従ずれの範囲Δｌは既知であるものとし、判定を行う（ステップＳ１５３）。

の場合には（ステップＳ１５３でＹｅｓ）、経路計画部１６は、移動体１が把持可能領域に到達可能であるものとして、直線ｍを用いて把持可能範囲に収まりやすい経路を生成する（ステップＳ１６）。ステップＳ１６の手順については、後に詳述する。

の場合には（ステップＳ１５３でＮｏ）、ステップＳ１５５に進む。

演算部１８は、移動体１の自己位置のばらつきの範囲の半径をΔｐと追従ずれの範囲Δlの合計値と、信頼楕円の長辺方向の長さとの比較を行う（ステップＳ１５４）。

の場合には（ステップＳ１５４でＹｅｓ）、ステップＳ１５５に進む。

の場合には（ステップＳ１５４でＮｏ）、経路計画部１６は、移動体１が検出範囲に確実に入る経路生成を行う（ステップＳ１７）。ステップＳ１７の手順については後に詳述する。

演算部１８は、

となるｃを計算し、目的地と把持対象物体とを結んだ直線と、直線ｍがなす角θの値を算出する（ステップＳ１５５）。図１１は、把持対象物体から目的地までを結んだ直線と、直線ｍとの関係を示した図である。

演算部１８は、角θがあらかじめ定めた閾値θ_ＴＨより小さいか否かを求める（ステップＳ１５６）。

である場合には（ステップＳ１５６でＮｏ）、直線ｍを用いて把持可能範囲に収まりやすい経路生成（ステップＳ１６）に進む。

である場合には（ステップＳ１５６でＹｅｓ）、移動体１の把持対象物体に対するアプローチが見た目上不自然となってしまうため、検出範囲確実に入る経路生成（ステップＳ１７）を行う。

ステップＳ１５において、移動体１が把持可能領域に到達可能であると判定されると、経路計画部１６は、把持可能範囲に収まりやすい経路生成を行う（ステップＳ１６）。経路計画部１６は以下の手順により経路を生成し、移動体１は経路に沿って移動する。図１２は、経路計画部１６が把持可能範囲に収まりやすい経路生成を行う詳細なフローチャートである。

経路計画部１６は、到達判定結果に応じた経路を作成する（ステップＳ１６１）。具体的には、経路計画部１６は、直線ｍに対して垂直な方向に経路点を数点配置し、これらの点と移動体１とを結ぶ経路を作成する。図１３Ａは、目的地から、直線ｍに対して垂直な方向に複数の経路点を配置した例であり、図１３Ｂは、複数の経路点を通り、移動体１と目的地を結ぶ経路計画を行った例である。

駆動制御部１７は、移動体１が目的地に到着するまで経路追従制御を行う（ステップＳ１６２）。

駆動制御部１７は、移動体１が把持対象物体に対して正面を向くように、目的地において旋回する（ステップＳ１６３）。その後、ステップＳ１８に進む。

移動体１は、自律移動を終了する（ステップＳ１８）。

ステップＳ１５において、移動体１が把持可能領域に到達可能でないと判定されると、経路計画部１６は、検出範囲に確実に入る経路生成を行う（ステップＳ１７）。経路計画部１６は以下の手順により経路を生成し、移動体１は経路に沿って移動する。図１４は、経路計画部１６が、検出範囲に確実に入る経路生成を行う、詳細なフローチャートである。

経路計画部１６は、目的地を検出可能領域の端に設定する（ステップＳ１７１）。具体的には、演算部１８は、把持可能領域の重心点を算出する。次に、経路計画部１６は、把持対象物体と重心点を結んだ直線と、検出可能領域の端線との交点のうち、把持対象物体から最も遠い点を目的地として設定する。図１５は、目的地の設定例である。

経路計画部１６は、移動体１の現在地から目的地に向かう経路を生成する（ステップＳ１７２）。具体的には、経路計画部１６は、把持対象物体と目的地とをつなぐ直線と平行方向に、目的地から数点の経路点を配置した後に、移動体１からの経路を生成する。図１６Ａは、目的地から数点の経路点を配置した状態であり、図１６Ｂは、目的地から数点の経路点を配置した後に、移動体１からの経路を生成した状態である。

駆動制御部１７は、目的地に移動するよう移動体１の移動を制御する（ステップＳ１７３）。

演算部１８は、目的地付近に移動体１が到着したときに、把持物体検出部１５により把持対象物体が検出できるか否かの判定を行う（ステップＳ１７４）。把持物体検出部１５により、把持対象物体が検出できない場合には（ステップＳ１７４でＮｏ）、駆動制御部１７は、把持物体検出部１５が把持対象物体を検出するまで、移動体１を前進させる（ステップＳ１７５）。なお、移動体１が目的地に到着したときに物体が検出できない場合とは、自己位置ばらつきや追従ずれの影響により、検出可能領域に自律移動体が入らない場合である。図１７Ａは、検出可能領域に移動体１が入らない場合の例であり、図１７Ｂは、移動体１が完全に検出可能領域に入るまで前進させた状態を示している。
把持物体検出部１５が、把持対象物体を検出した場合には（ステップＳ１７４でＹｅｓ）、ステップＳ１７６に進む。

駆動制御部１７は、把持対象物体と移動体１の相対位置関係に基づいて、移動体１の位置合わせを行う（ステップＳ１７６）。このとき移動体１の位置は、把持対象物体に対して、把持可能領域の重心よりも遠方となる。したがって駆動制御部１７は、移動体１を把持可能領域の重心に移動させる場合であっても、移動体１を把持対象物体と略反対方向に向けることは無い。図１８は、移動体１と把持可能領域重心と把持対象物体の位置関係、及び移動体１の移動方向を示した図である。その後、移動体１は移動を停止する（ステップＳ１８）。

したがって、把持可能範囲に収まりやすいと判定した場合には、移動体１の到達ラインとなる直線ｍを設けることにより、自己位置推定の精度が悪く移動体１の自己位置がばらつくことや、駆動制御部１７による経路追従の精度が悪く経路が外れることがあっても、移動体１が把持可能範囲の中に入ることを、ある程度保障することができる。
また、把持可能領域に収まりにくいと判定して経路生成を行う場合には、把持対象物体と把持可能領域重心を結んだ直線と、検出可能領域の端線との交点のうち、把持対象物体から遠い点を目的地とし、さらに把持対象物体と目的地を繋ぐ直線と平行方向に数点の経路点を配置することで、移動体１を確実に検出可能領域の中に入れることができる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、距離センサ１２として超音波距離計を用いることとしたが、レーザーを用いた距離センサなどの他の距離計測手法であっても良い。

１ 移動体
１１ ロボットアーム
１２ 距離センサ
１３ 環境地図データ記憶部
１４ 自己位置推定部
１５ 把持物体検出部
１６ 経路計画部
１７ 駆動制御部
１８ 演算部

Claims (1)

1. 移動体に設置された把持物体検出部が把持対象物体を検出することが可能な領域である物体検出可能領域と、前記移動体に設置されたロボットアームが前記把持対象物体に到達することが可能な領域である物体到達可能領域と、に基づいて、前記ロボットアームにより把持対象物体を把持可能な領域である物体把持可能領域を設定するステップと、
   前記移動体の自己位置ばらつきと、追従ずれと、に基づいて前記物体把持可能領域への到達可能性を判定するステップと、
   前記移動体を前記物体把持可能領域内に到達させる経路計画を生成するステップと、を備える、移動体の移動制御方法。

Images