JP4967458B2 - 経路作成装置及び経路作成方法 - Google Patents

Description

本発明は、ロボットの障害物を回避する動作経路を作成する経路作成装置に関する。

近年、各種産業用ロボットや人型ロボットなどの様々なロボットが開発されている。例えば、ロボットには、多数の関節を有し、関節間がリンクで連結され、各関節の動作によって多数の自由度を持つロボットがある。このようなロボットを動作させる場合、初期位置姿勢と目標位置姿勢が与えられ、初期位置姿勢から目標位置姿勢に至る経路を作成し、その経路に基づいてロボットの各関節を動作させる。さらに、このようなロボット全体を２次元平面内などで移動させるものもある。このようなロボットを動作や移動させる場合、ロボットの周辺に移動する障害物が存在すると、この移動障害物を回避するようにロボットを動作や移動させるための経路の作成は非常に難しくなる。これらロボットの経路の作成方法としては、例えば、３次元空間の中で複数の移動障害物を回避しながら移動する経路の計画方法であって、移動障害物の線形的な移動予測モデルに基づいて複数の移動経路を予め用意し、その複数の移動経路の中から選択することによって移動経路を作成する方法がある（非特許文献１参照）。この方法では、移動障害物とロボットとの衝突を回避するために、移動障害物とロボットを単純形状に近似することによって複数の移動経路について移動障害物とロボットとの干渉確認をそれぞれ行っている。
Ｊ．ＧＯ Ｔ．Ｖｕ Ｊ．Ｊ．Ｋｕｆｆｎｅｒ，Ａｕｔｏｎｏｍｏｕｓ Ｂｅｈａｖｏｉｒ ｆｏｒ Ｉｎｔｅｒａｃｔｉｖｅ Ｖｅｈｉｃｌｅ Ａｎｉｍａｔｉｏｎｓ，ＡＣＭ ＳＩＧＧＲＡＰＨ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｃｏｍｐｕｔｅｒ Ａｎｉｍａｔｉｏｎ，２００４

上記した従来の方法には、複雑な形状のロボットや移動障害物に対して移動経路の干渉確認を行う方法が提示されていない。複雑な形状の物体は、三角形の集まりやボクセルの集まりで近似され、複雑な形状のロボットを近似している三角形群あるいはボクセル群と複雑な形状の移動障害物を近似している三角形群あるいはボクセル群との三角形あるいはボクセルの各組み合わせによる干渉確認は非常に多くなり、処理量が増大する。そのため、ロボットと移動障害物との干渉確認に要する処理時間は、経路作成に要する全処理時間うちの大きなウエートを占める。特に、移動障害物の数が多い場合、干渉確認に要する処理量は更に増大する。したがって、オンラインによりリアルタイムでロボットを動作させる場合、リアルタイムでの経路作成が困難となり、実用上問題である。

そこで、本発明は、ロボットの障害物を回避する動作経路を高効率に作成する経路作成装置及び経路作成方法を提供することを課題とする。

本発明に係る経路作成装置は、ロボットの障害物を回避する動作経路を作成する経路作成装置であって、ロボットと障害物との干渉確認を行うための平面の数を設定する平面数設定手段と、障害物を認識する障害物認識手段と、平面数設定手段で設定した数分の各平面において、ロボットと障害物との干渉を確認する干渉確認手段と、干渉確認手段における確認結果に基づいて動作経路を作成する経路作成手段とを備え、平面数設定手段は、単位時間毎の所定間隔の各平面における障害物の断面形状から算出される障害物の断面形状の平面間の変化率及び／又は各平面における障害物の断面形状の時間の変化率に基づいて、平面の数を設定することを特徴とする。

この経路作成装置では、平面数設定手段により、ロボットと障害物との干渉確認を行うための平面の数を設定する。経路作成装置では、障害物認識手段により、障害物を認識する。そして、経路作成装置では、干渉確認手段により、設定した数分の各平面において、ロボットと認識した障害物との干渉確認を行う。つまり、障害物全体に対して干渉確認を行うのではなく、設定された数だけの平面における障害物の所定間隔毎の各部分に対して干渉確認を行う。さらに、経路作成装置では、経路作成手段により、その平面毎の干渉確認の結果に基づいて、障害物を回避する動作経路を作成する。このように、経路作成装置では、設定した数分の平面についてだけ干渉確認を行うので、干渉確認の処理量を低減することができる。そのため、経路を作成するときの処理量を大幅に低減でき、処理を高速化でき、ロボットの動作経路を高効率に作成することができる。特に、障害物の数が多い場合あるいは障害物の形状が複雑な場合、処理量を大幅に低減することができる。

なお、ロボットには、関節などによって動作する一般的なロボット以外にも、移動体も含むものとする。したがって、動作経路には、移動経路も含むものとする。

この経路作成装置の平面数設定手段では、所定間隔毎の平面毎に、各平面に交わった障害物の形状（平面による断面の形状）の変化に基づいて、平面の数を設定する。障害物の形状の変化としては、ある時間における平面間での形状の変化とある平面における単位時間毎の形状の変化がある。障害物の形状が複雑なほど、障害物における平面間の断面形状の変化が大きくなる。また、障害物の形状が複雑なほど、障害物を回避するためには障害物の多くの部分について干渉確認を行う必要がある。一方、障害物の形状が単純なほど、障害物における平面間の断面形状の変化が小さくなる。また、障害物の形状が単純なほど、障害物を回避するためには障害物の限られた部分での干渉確認でも十分がある。あるいは、障害物の移動や動作が激しいほど、障害物の単位時間毎の断面形状の変化が大きくなる。また、障害物の移動や動作が激しいほど、障害物を回避するためには障害物の多くの部分について干渉確認を行う必要がある。一方、障害物の移動や動作が少ないほど、障害物の単位時間毎の断面形状の変化が小さくなる。また、障害物の移動や動作が少ないほど、障害物を回避するためには障害物の限られた部分での干渉確認でも十分がある。したがって、各平面における障害物の形状の変化から平面の数を設定することができる。

本発明の上記経路作成装置では、平面数設定手段は、障害物の断面の両端間の長さの平面間の変化率又は各平面における障害物の断面の両端間の長さの時間の変化率が大きいほど平面の数を多くする構成としてもよい。

この経路作成装置の平面数設定手段では、所定間隔の平面毎に、各平面に交わった障害物の断面の両端間の長さを求め、その平面間の長さの変化が大きいほど平面の数を多く設定する。このように、経路作成装置では、障害物の断面形状の変化として断面の両端間の長さの変化という簡単に求めることができるパラメータを用いることにより、処理量を更に低減することができ、ロボットの動作経路をより高効率に作成することができる。

本発明に係る経路作成方法は、ロボットの障害物を回避する動作経路を作成する経路作成方法であって、ロボットと障害物との干渉確認を行うための平面の数を設定する平面数設定ステップと、障害物を認識する障害物認識ステップと、平面数設定ステップで設定した数分の各平面において、ロボットと障害物との干渉を確認する干渉確認ステップと、干渉確認ステップにおける確認結果に基づいて動作経路を作成する経路作成ステップとを含み、平面数設定ステップでは、単位時間毎の所定間隔の各平面における障害物の断面形状から算出される障害物の断面形状の平面間の変化率及び／又は各平面における障害物の断面形状の時間の変化率に基づいて、平面の数を設定することを特徴とする。

本発明の上記経路作成方法では、平面数設定ステップでは、障害物の断面の両端間の長さの平面間の変化率又は各平面における障害物の断面の両端間の長さの時間の変化率が大きいほど平面の数を多くする構成としてもよい。

なお、上記した各経路作成装置は、上記した各経路作成装置と同様の作用効果を奏する。

本発明は、設定した数分の各平面においてのみロボットと障害物との干渉確認を行うことにより、干渉確認の処理量を大幅に低減でき、ロボットの障害物を回避する動作経路を高効率に作成することができる。

以下、図面を参照して、本発明に係る経路作成装置及び経路作成方法の実施の形態を説明する。

本実施の形態では、本発明に係る経路作成装置及び経路作成方法を、多自由度リンク系の２次元平面を移動可能なロボットの動作経路を作成する経路作成装置に適用する。本実施の形態に係る経路作成装置は、ロボットが所定の位置から所定の位置までの動作するときに移動する障害物を回避するような動作経路を作成する。

図１〜図６を参照して、本実施の形態に係る経路作成装置１について説明する。図１は、本実施の形態に係る経路作成装置の構成図である。図２は、本実施の形態で適用されるロボットの一例である。図３は、本実施の形態で適用されるロボットの他の例である。図４は、ロボットと障害物との位置関係の一例を示す平面図である。図５は、ロボットと障害物との位置関係の一例を示す側面図である。図６は、本実施の形態に係る探索平面の数の設定方法の説明図である。

経路作成装置１は、ロボットの行動候補モデルを予め用意し、ロボットと移動障害物との干渉確認の結果に基づいて行動候補モデルを選択的につなげながらロボットが最終位置姿勢に至るまでの動作経路を作成する。特に、経路作成装置１は、処理負荷を軽減するために、ロボットと移動障害物との干渉確認を行うための平面の数を障害物の形状の変化に基づいて最適な数に設定する。そのために、経路作成装置１は、環境認識部２、データベース３、探索情報設定部４、断面算出部５、干渉確認部６、経路選択部７、経路出力部８を備えている。経路作成装置１の主要部はコンピュータ上あるいはロボット内の電子制御ユニットなどに構成され、特に、探索情報設定部４、断面算出部５、干渉確認部６、経路選択部７はハードディスクあるいはＲＯＭ内に格納された各アプリケーションプログラムをＲＡＭにロードし、ＣＰＵで実行することによって構成される。

なお、本実施の形態では、環境認識部２が特許請求の範囲に記載する障害物認識手段に相当し、探索情報設定部４が特許請求の範囲に記載する平面数設定手段に相当し、干渉確認部６が特許請求の範囲に記載する干渉確認手段に相当し、経路選択部７が特許請求の範囲に記載する経路作成手段に相当する。

まず、本実施の形態に適用されるロボットについて説明しておく。図２にはロボットの一例を示している。ロボットＲ１は、多数の関節Ｊ３，・・・，Ｊｎを備えており、関節間がリンクＬ３，・・・，Ｌｎ＋１で接続されている。また、ロボットＲ１は、末端のリンクＬ３の一端が台車Ｃに取り付けられ、先端のリンクＬｎ＋１の一端にハンドＨが取り付けられている。各関節Ｊ３，・・・，Ｊｎは、アクチュエータが内蔵されており、回転動作をそれぞれ行い、接続される２本のリンク間の角度Ｘ３，・・・，Ｘｎをそれぞれ変更する。台車Ｃは、駆動用モータと操舵用モータが設けられており、２次元平面上を移動し、ロボットＲ１の位置（Ｘ１，Ｘ２）を変更する。

このように、ロボットＲ１は、ｎ個の自由度を持つ。この自由度は、ｎ−２個の角度に対して座標軸及び２個の位置に対して座標軸を持つｎ次元座標空間（コンフィグレーション空間）における一点（Ｘ１，・・・，Ｘｎ）で表される。ロボットＲ１の先端部Ｔ（リンクＬｎ＋１とハンドＨとの取付部）の座標系（Ｙ１，Ｙ２，Ｙ３）は、コンフィグレーション空間から実際の３次元空間への非線形写像の値として、Ｙ１＝ｆ１（Ｘ１，・・・，Ｘｎ）、Ｙ２＝ｆ２（Ｘ１，・・・，Ｘｎ）、Ｙ３＝ｆ３（Ｘ１，・・・，Ｘｎ）で定義される。したがって、コンフィグレーション空間の（Ｘ１，・・・，Ｘｎ）を規定すると、（Ｙ１，Ｙ２，Ｙ３）が決まる。なお、ロボットＲ１や移動障害物が実際に移動する空間は、２次元空間や３次元空間であり、移動空間と呼ぶ。

また、図３にはロボットの他の例を示している。ロボットＲ２は、人型のロボットであり、左右一対のアーム部Ａ１，Ａ２とハンドＨ１，Ｈ２を有している。ロボットＲ２は、台車Ｃで２次元平面上を移動可能であり、１０個の関節Ｊ３，・・・，Ｊ１２を備えており、１２個の自由度を持つ。ロボットＲ２は、自由度がコンフィグレーション空間における座標系（Ｘ１，・・・，Ｘ１２）で表される。ロボットＲ２の場合、コンフィグレーション空間の（Ｘ１，・・・，Ｘ１２）を規定すると、各先端部Ｔ１，Ｔ２の座標系（Ｙ１１，Ｙ１２，Ｙ１３）、（Ｙ２１，Ｙ２２，Ｙ２３）が決まる。

それでは、経路作成装置１の各部について説明する。環境認識部２は、ロボットの周辺の環境情報（特に、障害物情報）を認識する手段である。特に、環境認識部２は、図５に示すように、ロボットＲの所定箇所から平面間隔角Δａ毎の探索平面Ｐ１，Ｐ２，Ｐ３に交わる障害物Ｏの情報を取得できる手段であればよい。環境認識部２としては、鉛直方向の平面間隔角Δａ毎に左右方向にスキャンしながら探索を行うセンサであり、例えば、ミリ波センサ、超音波センサ、レーザセンサ、レンジファインダである。環境認識部２では、探索情報設定部４で設定した平面間隔角Δａ毎の探索平面において、探索情報設定部４で設定した探索範囲ＳＡ内に存在する障害物の情報を取得する（図４，５参照）。また、環境認識部２では、障害物を認識できた場合、障害物の現在位置、移動速度、移動加速度、進行方向などを算出する。例えば、図３に示す人型ロボットの場合、顔部の目に相当する部分にミリ波センサなどが取り付けされる。

データベース３は、ハードディスクあるいはＲＡＭの所定の領域に構成される。データベース３には、ロボットに関する情報及び経路モデルが記憶される。ロボットに関する情報は、ロボット全体や各部の質量、ロボット全体や各部の形状及びサイズ、ロボットの重心、関節の回転角度範囲、リンク長などである。経路モデルに関する情報は、ロボットの動作特性や移動特性などを考慮して予め用意され、ロボットの自由度の各次元の行動モデルの候補からなる。

行動モデルについて説明する。ロボットのｎ次元の自由度をコンフィグレーション空間における座標系（Ｘ１，Ｘ２，・・・・，Ｘｎ）とし、そのうち（Ｘ１，Ｘ２）の２次元の自由度を２次元平面上の移動の自由度とする。行動モデルは、単位時間Δｔの間に１次元〜ｎ次元の各次元のコンフィグレーション空間でロボットを動作させた場合の行動の標準となるものである。ここでは、説明を容易するために、２次元平面上を移動する場合の２次元のコンフィグレーション空間での行動モデルについて説明する。この２次元行動モデルは、２次元コンフィグレーション空間の座標系（Ｘ１，Ｘ２）で表され、ｎ次元の自由度のうち２次元平面上の移動だけでロボットの動作（移動）を規定する行動モデルである。

ロボットは全方向（３６０°）に移動可能であるので、２次元行動モデルとして各方向に移動するための多数の候補モデルを設定可能である。したがって、データベース３には、２次元行動モデルについては、全方向に分布される所定数の行動候補モデルが記憶される。図４にはロボットＲからの各行動候補モデルＭ１，Ｍ２，Ｍ３の一例を示しているが、この以外に様々な方向への行動候補モデルがある。なお、関節における回転動作の場合、回転可能な範囲内で多数の候補モデルが設定可能である。

また、ロボットが移動する場合には速度や加速度を変化させることが可能なので、２次元行動モデルとして一定速度での移動、可変速度かつ一定加速度での移動、可変加速度での移動の候補モデルを設定可能である。したがって、データベース３には、２次元行動モデルについては、各方向の行動候補モデルについて３種類のモデル（一定速度のモデル、可変速度かつ一定加速度のモデル、可変加速度のモデル）がそれぞれ記憶される。

したがって、データベース３には、１次元〜ｎ次元の行動モデル毎に、方向の異なる所定数の行動候補モデルがそれぞれ記憶され、さらに、各方向の行動候補モデルについて３種類のモデル（一定速度のモデル、可変速度かつ一定加速度のモデル、可変加速度のモデル）がそれぞれ記憶される。いくつの方向の行動候補モデルを予め用意するかは、ロボットの移動動作や回転動作の特性、単位時間Δｔ、移動障害物の移動特性などを考慮して設定される。

探索情報設定部４は、環境認識部２で行う障害物の探索を行うために必要な情報を設定する手段である。この設定する情報としては、探索範囲ＳＡと平面間隔角Δａである。探索範囲ＳＡは、ロボットの最終位置の方向に向けて設定され、ロボットからの奥行き方向の範囲ｘ、ロボットを中心とした左右方向の範囲ｙ、高さ方向の範囲ｚで表される。奥行き方向の範囲ｘと左右方向の範囲ｙについては、ロボットＲを基準として２次元平面範囲角Δｂとその中心角ｃで表してもよい（図４参照）。高さ方向の範囲ｚについては、ロボットＲにおける環境認識部２の設置される箇所を基準として高さ範囲角Δｄとその中心角ｅで表してもよい（図５参照）。平面間隔角Δａは、高さ範囲ｚ（高さ範囲角Δｄ）内で障害物探索を行う探索平面の間隔（ひいては、探索平面の数）を規定する角度である。なお、探索平面の数は、ロボットと障害物との干渉確認を行う際の平面の数にも相当する。

探索情報設定部４では、動作開始時に、探索範囲ＳＡを設定するとともに、平面間隔角Δａの初期値を設定する。探索範囲ＳＡについては、オペレータによって入力されたものを設定してもよいし、あるいは、ロボットの位置や移動速度や移動加速度、ロボットの最終位置姿勢などを考慮して装置側で設定してもよい。また、平面間隔角Δａの初期値については、オペレータによって入力されたものを設定してもよいし、あるいは、ロボットの移動速度や移動加速度などを考慮して装置側で設定してもよい。

探索情報設定部４では、単位時間Δｔ毎に、断面算出部５で既に算出された単位時間Δｔ毎（・・・，ｔ−Δｔ，ｔ）の各探索平面（ｋ＝１，２，３，・・・）における障害物の断面形状に基づいて、現在の時間ｔにおける断面形状の探索平面間の変化率と各探索平面における断面形状の時間変化率をそれぞれ算出する。探索平面間変化率は、障害物の形状が複雑さを表し、変化率が大きいほど形状が複雑であることを示す。時間変化率は、障害物の移動や動作（ロボットの場合）が激しいさを表し、変化率が大きいほど移動や動作が激しいことを示す。

変化率を求める場合、障害物の断面における表面形状を表す厳密な形状によって求めてもよいし、あるいは、障害物の断面形状を表す簡易なパラメータによって求めてもよい。簡易なパラメータとしては、例えば、図６に示すように、各探索平面における障害物Ｏの断面のエッジ間の長さＬ１，Ｌ２，・・・を用いる。このエッジ間の長さを用いる場合、現在の時間ｔにおけるエッジ間の各長さＬ１，Ｌ２，・・・から探索平面間の変化率を算出するとともに、各探索平面について単位時間毎のエッジ間の各長さＬ１（ｔ），Ｌ１（ｔ−Δｔ），・・・から時間変化率を算出する。

探索情報設定部４では、探索平面間変化率又は時間変化率が、下限変化率以下か否かを判定するとともに上限変化率以上か否かを判定する。この下限変化率、上限変化率は、予め設定され、探索平面間変化率に対する値と時間変化率に対する値が同一の値でもよいしあるいは異なる値でもよい。

探索平面間変化率又は時間変化率が上限変化率以上の場合、探索情報設定部４では、平面間隔角Δａとして小さい値を設定する。この場合、障害物の形状が複雑又は障害物の移動や動作が激しいので、探索平面数を増加し、障害物の探索や干渉確認をより多くの部分に対して行う。探索平面間変化率及び時間変化率が下限変化率以下の場合、探索情報設定部４では、平面間隔角Δａとして大きい値を設定する。この場合、障害物の形状が単調かつ障害物の移動や動作が少ないので、探索平面数を減少し、障害物の探索や干渉確認をより少なく部分に対して行う。これら以外の場合、探索情報設定部４では、平面間隔角Δａとして初期値を維持する。なお、平面間隔角Δａを小さくする場合あるいは大きくする場合、予め設定した一定の角度だけ変化させるようにしてもよいし、あるいは、変化率の増減量に応じて変化させる角度を可変設定してもよい。

断面算出部５は、各平面における障害物の断面形状と各平面におけるロボットの断面形状を算出する手段である。断面算出部５では、単位時間Δｔ毎に、環境認識部２で取得した探索平面毎の障害物情報に基づいて、現在の時間ｔから単位時間Δｔ経過後までの各探索平面に交わる障害物の断面の表面形状をそれぞれ算出する。この際、障害物の移動速度や移動加速度及び進行方向から予想経路を算出し、予測経路から単位時間Δｔを経過する過程の障害物の位置を算出する。また、断面算出部５では、単位時間Δｔ毎に、データベース３に記憶されているロボットの形状情報に基づいて、データベース３に記憶されている各行動候補モデルについて現在の時間ｔから各行動候補モデル分動作したときの（現在の時間ｔから単位時間Δｔ経過後までの）各平面に交わるロボットの断面の表面形状をそれぞれ算出する。ロボットについては、探索平面に交じる障害物の高さ位置における水平面に交わるロボットの断面である。なお、障害物がロボットであり、関節などが動作する場合、障害物の動作を予想し、動作も考慮して現在の時間ｔから単位時間Δｔ経過後までの探索平面に交わる障害物の断面の表面形状を算出する。

干渉確認部６は、データベース３に記憶されている行動候補モデル毎にロボットと障害物との干渉を確認する手段である。干渉確認部６では、行動候補モデル毎に、各平面について、断面算出部５で算出したロボットの現在の時間ｔから単位時間Δｔ経過後までの断面形状と断面算出部５で算出した障害物の現在の時間ｔから単位時間Δｔ経過後までの断面形状との干渉確認をそれぞれ行う。つまり、各平面（探索平面に交じる障害物の高さ位置における水平面）において、ロボットの現時点位置姿勢から行動候補モデル分動作したときの位置姿勢までの単位時間Δｔ毎の断面形状と障害物の現在位置から単位時間Δｔ分移動した位置までの単位時間Δｔ毎の断面形状とが干渉するか否かを判定する。干渉の確認方法としては、例えば、ロボットの断面形状と移動物体の断面形状との相対距離を算出し、その相対距離が大きいほど干渉する可能性が低く、相対距離が０以下になると干渉状態と判定する。

経路選択部７は、データベース３に記憶されている行動候補モデルの中から最適なモデルを選択し、その選択した行動候補モデルにより動作経路を作成する手段である。経路選択部７では、行動候補モデルの中から、干渉確認部６で干渉確認の結果に基づいて最も干渉する可能性の低いモデルを選択する。相対距離で干渉確認を行う場合、行動候補モデルの中から、全ての平面について相対距離が０より大きく（干渉無し）かつ全ての平面についての相対距離の平均値が最も大きい行動候補モデルを選択する。さらに、経路選択部７では、この選択した行動候補モデルを用いて単位時間Δｔ後までの動作経路を作成する。経路選択部７では、ロボットの最終位置姿勢に到達するまで、以上の処理を単位時間Δｔ毎に繰り返し行う。これによって、経路選択部７では、最終位置姿勢までの動作経路を作成する。

経路出力部８は、経路選択部７で作成した動作経路を出力する手段である。経路出力部８は、例えば、モニタ、プリンタ、ロボットを動作させる制御部との通信を行う通信装置である。また、経路出力部８は、ロボットを動作させる制御部としての機能を有する場合、動作経路における各位置姿勢のコンフィグレーション空間の座標系（台車の位置や各関節の角度）に従ってロボットの各関節のアクチュエータを駆動制御する。

なお、行動候補モデルを用いて処理を行う処理部では、データベース３に記憶されている全ての行動候補モデルについて各処理を行うようにしてもよいし、あるいは、データベース３から所定条件を満たす行動候補モデルを抽出し、その抽出した行動候補モデルについて各処理を行うようにしてもよい。所定条件としては、例えば、探索範囲ＳＡ内に含まされる行動候補モデル、ロボットの最終位置の方向を中心した所定範囲内に含まれる行動候補モデルである。

図１を参照して、経路作成装置１の動作を説明する。特に、探索平面の数を設定する処理については、図７のフローチャートに沿って説明する。図７は、本実施の形態に係る探索平面の数を設定するための処理の流れを示すフローチャートである。

経路作成装置１には、オペレータによって最終位置姿勢が入力される。動作開始時、探索情報設定部４では、探索範囲ＳＡを設定するとともに（Ｓ１）、平面間隔角Δａに初期値を設定する（Ｓ２）。また、経路作成装置１では、オペレータによる入力値などによって単位時間Δｔを設定する（Ｓ３）。

動作開始後、環境認識部２では、探索情報設定部４で設定した探索範囲ＳＡと平面間隔角Δａを用いて、平面間隔角Δａ毎の各探索平面において探索範囲ＳＡ内に存在する障害物の探索を行い、探索平面毎の障害物の情報を取得する。この環境認識部２で取得した探索平面毎の障害物情報を用いて、断面算出部５では、単位時間Δｔ毎に、現在の時間ｔから単位時間Δｔ経過後までの各探索平面に交わる障害物の断面形状をそれぞれ算出する（Ｓ４）。また、データベース３に記憶されているロボットの形状情報を用いて、断面算出部５では、データベース３に記憶されている行動候補モデル毎に、現在の時間ｔから各行動候補モデル分動作するまでの各平面に交わるロボットの断面形状をそれぞれ算出する。

探索情報設定部４では、単位時間Δｔ毎に、断面算出部５で既に算出された単位時間Δｔ毎の各探索平面に交わる障害物の断面形状に基づいて、現在の時間ｔにおける探索平面間の断面形状の変化率と各探索平面における単位時間Δｔ間の断面形状の変化率をそれぞれ算出する（Ｓ５）。そして、探索情報設定部４では、探索平面間の変化率と単位時間Δｔ間の変化率が、下限変化率以下か否かを判定するとともに上限変化率以上か否かを判定する（Ｓ６）。

Ｓ６にて探索平面間の変化率又は各探索平面における単位時間Δｔ間の変化率が上限変化率以上と判定した場合、探索情報設定部４では、探索平面の数を増加するために、平面間隔角Δａに小さな角度を設定する（Ｓ７）。Ｓ６にて探索平面間の変化率及び各探索平面における単位時間Δｔ間の変化率が下限変化率以下と判定した場合、探索情報設定部４では、探索平面の数を減少するために、平面間隔角Δａに大きな角度を設定する（Ｓ８）。Ｓ６にてそれら以外と判定した場合、探索情報設定部４では、探索平面の数を維持するために、平面間隔角Δａを維持する（Ｓ９）。この設定された平面の数により、次回の障害物探索や干渉確認を行う。したがって、平面の数が増加する場合にはより厳密な探索な干渉確認が行われ、平面の数が減少する場合にはより簡単な探索や干渉確認が行われる。

干渉確認部６では、行動候補モデル毎に、断面算出部５で算出したロボットの現在の時間ｔから単位時間Δｔ経過後までの断面形状及び障害物の現在の時間ｔから単位時間Δｔ経過後までの断面形状に基づいて、各平面についてロボットと障害物とが干渉するか否かの確認を行う。そして、経路選択部７では、干渉確認部６での干渉確認結果に基づいて、データベース３に記憶されている行動候補モデルの中から最も干渉の可能性の低い行動候補モデルを選択する。さらに、経路選択部７では、この選択した行動候補モデルを用いて動作経路を作成する。経路出力部８では、この経路選択部７で作成した動作経路を出力する。

経路作成装置１では、ロボットが最終位置姿勢に到達したか否かを判定する（Ｓ１０）。Ｓ１０にて最終位置姿勢に到達していないと判定した場合、経路作成装置１では、Ｓ４に戻り、上記の処理を単位時間Δｔ毎に繰り返し行う。一方、Ｓ１０にて最終位置姿勢に到達したと判定した場合、経路作成装置１では、処理を終了する。これによって、移動する障害物を回避する最終位置姿勢までの動作経路が作成される。

この経路作成装置１によれば、平面の最適な数を設定し、その数分の平面についてだけ障害物探索や干渉確認を行うので、障害物探索及び干渉確認の処理負荷を軽減することができる。そのため、動作経路を作成するときの処理量を大幅に低減でき、処理を高速化でき、ロボットの動作経路を高効率に作成することができる。特に、障害物の数が多い場合あるいは障害物の形状が複雑な場合、処理量を大幅に低減することができる。

経路作成装置１では、平面の数を設定するために平面間の断面形状の変化率と単位時間Δｔ間の断面形状の変化率を用いるので、障害物の形状の複雑さ及び障害物の移動や動作の激しさの両面から平面の数を設定することができる。したがって、障害物の形状が単調な場合や障害物の移動や動作が少ない場合には、障害物探索や干渉確認の処理量を低減しつつ障害物を回避する動作経路を確実に作成することができる。また、障害物の形状が複雑な場合や障害物の移動や動作が激しい場合には、障害物探索や干渉確認をより厳密に行うことができ、障害物を回避する動作経路を確実に作成することができる。

経路作成装置１では、断面形状の変化率を求めるために平面における障害物の断面のエッジ間の長さを用いるので、この簡単に求めることができるパラメータを用いることにより、処理量を更に低減することができ、ロボットの動作経路をより高効率に作成することができる。

以上、本発明に係る実施の形態について説明したが、本発明は上記実施の形態に限定されることなく様々な形態で実施される。

例えば、本実施の形態では多数の関節を有し、関節間がリンクで連結され、各関節の回転動作によって多数の自由度を持ちかつ２次元平面を移動するロボットに適用したが、関節が伸縮動作などの他の動作を行うものでも適用可能であり、１次元的に移動するロボットや３次元空間を移動するロボットでも適用可能であり、ロボット全体が移動しないものでも適用可能であり、あるいは、関節を持たない単純な移動体にも適用可能である。移動体の場合、作成する経路としては移動経路となる。

また、本実施の形態では移動する障害物の場合に適用したが、移動しない障害物の場合にも適用可能である。

また、本実施の形態では障害物認識手段として左右方向にスキャンしながら探索を行うミリ波レーダなどを適用し、障害物の各探索平面に交わる情報を取得し、この探索平面毎に取得した情報により干渉確認を行う構成としたが、カメラなどの手段によって障害物全体の情報を取得し、干渉確認を行う段階で取得した障害物の全体情報から平面毎の情報を抽出し、その抽出した平面毎の情報により干渉確認を行う構成としてもよい。

また、本実施の形態では行動候補モデルを予め用意してデータベースに記憶させ、行動候補モデルの中から選択して経路を作成する構成としたが、確率的手法や計算などによって経路を作成する構成としてもよい。

また、本実施の形態では各探索平面における障害物の断面のエッジ間の長さの変化に基づいて探索平面の数を設定する構成としたが、断面のエッジ間の長さ以外の他の情報を用いて障害物の形状の変化から平面の数を設定してもよいし、あるいは、他の手法によって平面の数を設定してもよい。

また、本実施の形態では障害物の断面形状の探索平面間の変化率と断面形状の時間の変化率を用いて探索平面の数を設定する構成としたが、探索平面間の変化率だけで設定する構成としてもよい（例えば、障害物が移動や動作しないと予め判っている場合）、あるいは、時間の変化率だけで設定する構成としてもよい（例えば、障害物の形状が予め判っている場合）。

本実施の形態に係る経路作成装置の構成図である。 本実施の形態で適用されるロボットの一例である。 本実施の形態で適用されるロボットの他の例である。 ロボットと障害物との位置関係の一例を示す平面図である。 ロボットと障害物との位置関係の一例を示す側面図である。 本実施の形態に係る探索平面の数の設定方法の説明図である。 本実施の形態に係る探索平面の数を設定するための処理の流れを示すフローチャートである。

符号の説明

１…経路作成装置、２…環境認識部、３…データベース、４…探索情報設定部、５…断面算出部、６…干渉確認部、７…経路選択部、８…経路出力部

Claims (4)

1. ロボットの障害物を回避する動作経路を作成する経路作成装置であって、
   ロボットと障害物との干渉確認を行うための平面の数を設定する平面数設定手段と、
   障害物を認識する障害物認識手段と、
   前記平面数設定手段で設定した数分の各平面において、ロボットと障害物との干渉を確認する干渉確認手段と、
   前記干渉確認手段における確認結果に基づいて動作経路を作成する経路作成手段と
   を備え、
   前記平面数設定手段は、単位時間毎の所定間隔の各平面における障害物の断面形状から算出される障害物の断面形状の平面間の変化率及び／又は各平面における障害物の断面形状の時間の変化率に基づいて、平面の数を設定することを特徴とする経路作成装置。
2. 前記平面数設定手段は、障害物の断面の両端間の長さの平面間の変化率又は各平面における障害物の断面の両端間の長さの時間の変化率が大きいほど平面の数を多くすることを特徴とする請求項１に記載する経路作成装置。
3. ロボットの障害物を回避する動作経路を作成する経路作成方法であって、
   ロボットと障害物との干渉確認を行うための平面の数を設定する平面数設定ステップと、
   障害物を認識する障害物認識ステップと、
   前記平面数設定ステップで設定した数分の各平面において、ロボットと障害物との干渉を確認する干渉確認ステップと、
   前記干渉確認ステップにおける確認結果に基づいて動作経路を作成する経路作成ステップと
   を含み、
   前記平面数設定ステップでは、単位時間毎の所定間隔の各平面における障害物の断面形状から算出される障害物の断面形状の平面間の変化率及び／又は各平面における障害物の断面形状の時間の変化率に基づいて、平面の数を設定することを特徴とする経路作成方法。
4. 前記平面数設定ステップでは、障害物の断面の両端間の長さの平面間の変化率又は各平面における障害物の断面の両端間の長さの時間の変化率が大きいほど平面の数を多くすることを特徴とする請求項３に記載する経路作成方法。

Images