JP7560874B2 - 経路生成方法及び経路生成装置 - Google Patents

Description

本発明は、経路生成方法及び経路生成装置に関する。

近年、人口の減少に伴い生産現場における労働力の不足が進んでいる。労働力の不足を補うひとつの手段として、生産支援ロボットの導入が検討されている。特許文献１、２は、組み立て用ロボットに関する技術を開示する。生産支援ロボットが動作するときには、ロボットの周囲に存在する障害物との衝突が発生する可能性がある。そこで、障害物との衝突を回避するようにロボットを制御する必要がある。特許文献３～５は、このようなロボットの制御に関する技術を開示する。

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

所定の条件を満たすようにロボットを動作させる技術のひとつに、人間がロボットに動作を教える手法がある。このような手法は、教示型と呼ばれている。教示型は、ロボットに対して直感的に動作を教えることができるので、容易である。しかし、条件を満たす動作を生成するために要する試行回数が増えると、教示に必要な時間も増加する。

別の手法として、条件を満たす動作をロボットが自ら生成する技術もある。このような手法は、自律型と呼ばれている。自律型の手法によれば、動作の生成は環境モデルを設定すれば自動で動作生成を行うことができる。しかし、ロボットが適用される環境は、複雑である。つまり、動作の生成には、環境モデルが必要であるが、環境モデルの準備には時間を要する。

本発明は、経路の生成に要する時間を短縮することができる経路生成方法及び経路生成装置を提供することを目的とする。

本発明の一形態は、移動物体が障害物に衝突することなく出発点から最終目標点にまで至る到達可能経路を生成する経路生成方法であって、移動物体が障害物に衝突したことを検知する工程と、衝突が検知された場合に、衝突が発生した位置に関する衝突位置情報と衝突によって移動物体が受けた反力の向きに関する反力方向情報とを記録する工程と、衝突位置情報、反力方向情報、及び反力の向きに向かって経路の一部を移動させる量を示す修正パラメータを利用して、衝突を生じさせないように経路を修正する工程と、を含み、検知する工程、記録する工程及び修正する工程を繰り返して、到達可能経路を生成する生成工程と、生成工程の後に、衝突位置情報及び反力方向情報を利用して、生成工程において到達可能経路が生成されるまでに要した繰り返し数よりも少ない繰り返し数で到達可能経路が生成されるように、修正パラメータを調整する調整工程と、を有する。

この方法によれば、到達可能経路を生成するごとに、修正パラメータが修正される。その結果、到達可能経路を生成するごとに、到達可能経路の生成に要する繰り返し数が少なくなるので、経路の生成に要する時間を短縮することができる。

一形態において、経路を修正する工程では、予め準備された修正パラメータを読み出す工程を含み、修正パラメータは、衝突が発生した位置における経路の方向であって衝突位置情報から導かれる経路方向情報及び反力方向情報の組み合わせに紐づけられて予めデータベースに記憶されてもよい。この工程によれば、到達可能経路を生成する環境が互いに異なる場合であっても、調整された修正パラメータを、別の環境における到達可能経路の生成に適用することができる。

一形態において、修正パラメータを読み出す工程では、記録する工程の結果として得られた衝突位置情報及び反力方向情報を用いて、経路方向情報及び反力方向情報の組み合わせを導出し、当該組み合わせと、データベースに記憶されている組み合わせとをマッチングさせることによって、修正パラメータを読み出してもよい。この工程によれば、より好適に調整された修正パラメータを、別の環境における到達可能経路の生成に適用することができる。

一形態において、予め準備された修正パラメータは、第ｎの環境における第ｎの到達可能経路を生成した後に調整された第ｎの修正パラメータであり、第ｎの環境とは異なる第ｎ＋１の環境において第ｎ＋１の到達可能経路を生成するとき、第ｎの修正パラメータを用いると共に、第ｎ＋１の到達可能経路を生成したのちに第ｎの修正パラメータを調整して第ｎ＋１の修正パラメータを得てもよい。この方法によれば、到達可能経路を生成するごとに、修正パラメータを調整することができる。

生成工程は、経路を規定する制御点の追加または削除の要否を判定する判定工程をさらに含んでもよい。この工程によれば、より好適な到達可能経路を生成することができる。

本発明の別の形態は、移動物体が障害物に衝突することなく出発点から最終目標点にまで至る到達可能経路を生成する経路生成装置である。経路生成装置は、衝突が発生した位置に関する衝突位置情報及び衝突によって移動物体が受けた反力の向きに関する反力方向情報を受け入れる情報取得部と、衝突位置情報及び反力方向情報を利用して、到達可能経路を生成する少なくとも１個のプロセッサと、を備える。少なくとも１個のプロセッサは、移動物体が障害物に衝突したことを検知する動作と、衝突が検知された場合に、衝突が発生した位置に関する衝突位置情報と衝突によって移動物体が受けた反力の向きに関する反力方向情報とを記録する動作と、衝突位置情報、反力方向情報、及び反力の向きに向かって経路の一部を移動させる量を示す修正パラメータを利用して、衝突を生じさせないように経路を修正する動作と、を含み、検知する動作、記録する動作及び修正する動作を繰り返して、到達可能経路を生成し、到達可能経路を生成した後に、衝突位置情報及び反力方向情報を利用して、到達可能経路が生成されるまでに要した繰り返し数よりも少ない繰り返し数で到達可能経路が生成されるように、修正パラメータを調整する。

この装置によれば、到達可能経路を生成するごとに、修正パラメータが修正される。その結果、到達可能経路を生成するごとに、到達可能経路の生成に要する繰り返し数が少なくなるので、経路の生成に要する時間を短縮することができる。

本発明の経路生成方法及び経路生成装置によれば、経路の生成に要する時間を短縮することができる。

図１は、実施形態の経路生成方法を実行する経路生成装置が適用される様子を示す図である。 図２は、経路生成装置を実現するコンピュータの構成図である。 図３は、初期経路から到達可能経路を生成する様子を示す図である。 図４は、経路修正の概念を示す図である。 図５は、実施形態の経路生成方法の主要な工程を示すフロー図である。 図６（ａ）は、失敗点の付近を拡大して経路方向及び反力方向を示す図である。図６（ｂ）は、ＲＯＭに記録されている情報を模式的に示す図である。 図７は、実施形態の経路生成装置の機能ブロックを示す図である。 図８は、コスト関数の分布を示すグラフの一例である。

以下、図面を参照しつつ本発明に係る経路生成方法及び経路生成装置の実施形態について詳細に説明する。なお、図面の説明においては、同一又は相当部分には同一符号を付し、重複する説明を省略する。

図１に示すロボットシステム１００は、経路生成方法を実行する経路生成装置の適用先の例示である。ロボットシステム１００は、マニピュレータ１０と、コンピュータ２０（経路生成装置）と、を有する。コンピュータ２０によって所定のプログラムが実行されることにより、経路生成装置が実現される。

マニピュレータ１０は、ベース１１と、複数のリンク１２と、複数のジョイント１３と、エンドエフェクタ１４（移動物体）と、を有する。マニピュレータ１０は、コンピュータ２０から送信される制御信号によってジョイント１３の動作が制御される。ジョイント１３の動作した結果、先端に設けられたエンドエフェクタ１４が経路Ｒに沿って移動する。つまり、コンピュータ２０は、エンドエフェクタ１４が経路Ｒに沿って移動するように、ジョイント１３の動作を制御する。

ジョイント１３は、駆動力を発生させるモータ１３ａを含む。さらに、ジョイント１３は、モータ１３ａの回転角度に関する情報を出力するロータリエンコーダ１３ｂを含む。ロータリエンコーダ１３ｂが出力する情報を処理することによって、エンドエフェクタ１４の位置情報を得ることができる。エンドエフェクタ１４は、力覚センサ１４ａを含む。力覚センサ１４ａは、エンドエフェクタ１４が障害物２００に衝突したときに、障害物２００から受ける接触力に関する情報を出力する。接触力に関する情報は、接触力の方向と、接触力の大きさとを含んでよい。以下の説明において、衝突が発生した位置を「失敗点ｆ」と称する。つまり、「失敗点ｆ」は、２次元座標系又は３次元座標系を基準とした位置を示す。さらに、接触力の方向に関する情報は、「修正ベクトルｍ」と称する。つまり、「修正ベクトルｍ」は、２次元座標系又は３次元座標系を基準として、接触力の向きを示す。本実施形態における「修正ベクトルｍ」は方向の情報だけを含み、大きさの情報は含まない。つまり、修正ベクトルｍは単位ベクトルであるともいえる。

コンピュータ２０は、マニピュレータ１０に対して信号を送受信可能に接続される。コンピュータ２０とマニピュレータ１０との接続構成は、特に限定されない。コンピュータ２０とマニピュレータ１０とは、有線で接続されてもよい。コンピュータ２０とマニピュレータ１０とは、無線で接続されてもよい。さらには、コンピュータ２０とマニピュレータ１０とは、インターネットなどの通信回線を介して接続されてもよい。

コンピュータ２０の構成も特に限定されない。コンピュータ２０は、１台の装置によって構成されてもよい。コンピュータ２０は、複数の装置によって構成されてもよい。複数の装置は、有線又は無線のネットワークを介して相互に情報通信可能に接続されて構成されてもよい。

図２は、コンピュータ２０のハードウェア構成の例示である。コンピュータ２０は、物理的には、プロセッサ２０ａ、記録媒体であるＲＡＭ（Random Access Memory）２０ｂ又はＲＯＭ（Read Only Memory）２０ｃ、通信モジュール２０ｄ、及び入出力モジュール２０ｅ等を含んだコンピュータ等であり、各々は電気的に接続されている。なお、コンピュータ２０は、入出力モジュール２０ｅとして、ディスプレイ、キーボード、マウス、タッチパネルディスプレイ等を含んでいてもよいし、ハードディスクドライブ、半導体メモリ等の情報記録装置を含んでいてもよい。

コンピュータ２０は、マニピュレータ１０を動作させるための制御信号を送信する。また、コンピュータ２０は、マニピュレータ１０から送信される各種情報を受信する。コンピュータ２０は、受信した情報と予め準備された情報など利用して、制御信号を生成する。具体的には、コンピュータ２０は、エンドエフェクタ１４を出発点ＰＳから最終目標点ＰＧまで障害物２００に衝突することなしに到達できる経路Ｒを生成する。コンピュータ２０は、プログラムを実行することによって実現されるいくつかの機能構成要素を含む。これらの機能構成要素については、後に詳細に説明する。

図３に示すように、コンピュータ２０は、出発点ＰＳから最終目標点ＰＧに至る初期経路ＲＳを設定する。そして、コンピュータ２０は、初期経路ＲＳに沿ってエンドエフェクタ１４を移動させる。初期経路ＲＳが障害物２００と交差する部分（失敗点ｆ１、ｆ２）を含む場合に、エンドエフェクタ１４は、障害物２００に衝突する。衝突したとき、コンピュータ２０は、力覚センサ１４ａから反力方向情報を受信する。そして、コンピュータ２０は、受信した情報を用いて、衝突を回避する新たな経路Ｒを再設定する。コンピュータ２０は、エンドエフェクタ１４を移動させる動作と、力覚センサ１４ａから情報を受信する動作と、経路Ｒを再設定する動作と、を繰り返すことによって、出発点ＰＳから最終目標点ＰＧまで衝突なしに移動できる到達可能経路ＲＰを生成する。

コンピュータ２０は、衝突が発生した場合に、失敗点ｆ、修正ベクトルｍ及び修正パラメータβを用いて、衝突を発生させない新たな経路を生成する。失敗点ｆ及び修正ベクトルｍは、前述のとおりである。図４は、失敗点ｆにおける経路修正の概念を示す。図４に示すように、失敗点ｆにおいて衝突が発生した場合には、修正ベクトルｍの方向に経路を移動させればよい。その結果、障害物２００と交差しない新たな経路Ｒを設定できる。この修正ベクトルｍは、方向のみを示す情報であるから、力覚センサ１４ａから受信する情報に基づいて決定できる。つまり、修正ベクトルｍが示す方向は、反力の方向である。しかし、失敗点ｆを修正ベクトルｍの方向に、どの程度移動させるかについては、一意に決定できない。そこで、修正ベクトルｍの方向にどの程度移動させるかを示す指標として、修正パラメータβを導入する。修正パラメータβの値が大きいとき、退避距離は大きい。修正パラメータβの値が小さいとき、退避距離は小さい。周囲の環境によって、修正パラメータβが大きいほうが望ましい場合もあれば、大きすぎる修正パラメータβは新たな失敗点ｆを生む要因にもなり得る。そのため、好適な修正パラメータβの設定は、負荷の高い作業である。

修正パラメータβは、人間の教示によって示される場合がある。しかし、人間の教示による修正パラメータβの設定は、人間の負荷の増大につながる。そこで、本願発明者らは、ある修正パラメータβを用いて経路の自動生成を行った結果を利用して、修正パラメータβを調整することにより、より好適な調整された修正パラメータβ_ＮＥＷを得る技術に想到した。より好適とは、より少ない試行回数で、目的の経路を設定できることを意味する。つまり、調整された修正パラメータβ_ＮＥＷを用いることにより、目的の経路をより少ない試行回数で得ることができる。

以下、コンピュータ２０が実行する経路生成方法を図５に示すフロー図を参照しながら説明する。経路生成方法は、経路の自動生成を行う動作を含む。さらに、経路生成方法は、経路の自動生成を行う動作において得た各種の値を用いて、修正パラメータβを調整して、調整された修正パラメータβ_ＮＥＷを得る動作を含んでもよい。そして、以下の説明では、二次元平面において経路Ｒの設定を行うものとする。しかし、経路生成方法は、二次元の経路Ｒの設定に適用できるだけでなく、三次元空間における経路Ｒの設定にも適用できる。

＜経路の自動生成＞
コンピュータ２０は、初期経路ＲＳを設定する（工程Ｓ１）。初期経路ＲＳは、少なくとも、出発点ＰＳと最終目標点ＰＧとによって規定される。初期経路ＲＳは、出発点ＰＳ及び最終目標点ＰＧに加えて、制御点Ｐａ及び固定経由点Ｐｎを含む。経路Ｒは、直線部分や曲線部分の組み合わせによって構成されている。経路Ｒは、規定された点群のスプライン補間などのパラメータ曲線の演算によって得られる。制御点Ｐａは、これらの直線部分や曲線部分を規定する点である。経路Ｒの修正は、例えば、制御点Ｐａの位置を変更することによって、経路Ｒの一部を移動することができる。一方、固定経由点Ｐｎは、出発点ＰＳから最終目標点ＰＧに移動する間に必ず通過すべき点である。経路Ｒが変更されたとしても、固定経由点Ｐｎの位置は変更されない。固定経由点Ｐｎは、必要に応じて設定してよく、固定経由点Ｐｎを含まない経路Ｒもあり得る。初期経路ＲＳは、人間が教示してもよい。

次に、コンピュータ２０は、経路の修正動作を開始する（工程Ｓ２）。そして、コンピュータ２０は、追従制御を開始する（工程Ｓ３）。追従制御とは、エンドエフェクタ１４を経路Ｒに沿って移動させる制御をいう。コンピュータ２０は、経路Ｒを示す情報に基づいて、マニピュレータ１０のモータ１３ａを駆動させる制御信号を生成する。そして、生成した制御信号をマニピュレータ１０に送信する。

コンピュータ２０は、追従制御と並行して、衝突の有無を判定する動作を行う（工程Ｓ）。コンピュータ２０は、衝突が発生していないと判定したとき（工程Ｓ４：ＮＯ）、エンドエフェクタ１４が最終目標点ＰＧに到達したかを判定する（工程Ｓ５）。コンピュータ２０は、エンドエフェクタ１４が最終目標点ＰＧに到達していないと判定したとき（工程Ｓ５：ＮＯ）、再び工程Ｓ３を行う。つまり、コンピュータ２０は、エンドエフェクタ１４の衝突又はエンドエフェクタ１４の最終目標点ＰＧへの到達のいずれかが発生するまで、工程Ｓ３、工程Ｓ４及び工程Ｓ５を繰り返す。この繰り返しにおいて、エンドエフェクタ１４が最終目標点ＰＧへ到達したと判定されたとき（工程Ｓ５：ＹＥＳ）、コンピュータ２０は経路修正動作を終了する（工程Ｓ１０）。そして、コンピュータ２０は、修正パラメータβの調整を行う（工程Ｓ１１）。工程Ｓ１１の詳細は、後述する。

コンピュータ２０は、エンドエフェクタ１４に設けた力覚センサ１４ａから衝突を示す情報を受信したとき、衝突が発生したと判定する。例えば、力覚センサ１４ａから受信した情報において接触力の大きさを示す値が所定値よりも大きいとき、衝突が発生したと判定してもよい。コンピュータ２０は、衝突が発生したと判定しないかぎり（工程Ｓ４：ＮＯ）、追従制御（工程Ｓ３）を継続する。コンピュータ２０は、衝突が発生したと判定したとき（工程Ｓ４：ＹＥＳ）、経路Ｒを再設定する動作を行う（工程Ｓ６、Ｓ７、Ｓ８、Ｓ９）。少なくとも工程Ｓ４、Ｓ６、Ｓ８、Ｓ９は、経路を生成する工程Ｓ２０を構成する。

コンピュータ２０は、衝突が発生したと判定したとき（工程Ｓ４：ＹＥＳ）、失敗点ｆの位置と、修正ベクトルｍの方向と、を取得する（工程Ｓ６）。失敗点ｆの位置は、例えば、ロータリエンコーダ１３ｂから受信した情報を利用して算出してもよい。修正ベクトルｍの方向は、力覚センサ１４ａから受信した情報を用いて得てもよい。

コンピュータ２０は、制御点Ｐａの追加を要するか否かを判定する（工程Ｓ７）。なお、コンピュータ２０は、制御点Ｐａの削除が可能であるか否かも併せて判定してもよい。例えば、コンピュータ２０は、各制御点Ｐａに対して、それらの制御点Ｐａが影響を及ぼす評価（失敗点ｆとそれに対応する修正ベクトルｍ）を行う。そして、複数の評価の間に齟齬が生じる度合いが大きい場合に、新たな制御点Ｐａを追加すると判定する。換言すると、コンピュータ２０は、複数の評価の修正ベクトルｍの違いを定量化する。そして、コンピュータ２０は、定量化された修正ベクトルｍの違いの度合いが大きい場合に、新たな制御点Ｐａを追加すると判定する。例えば、近接する複数の失敗点ｆで、その修正ベクトルｍの方向が大きく（所定量以上）異なり、失敗点ｆに対応する既存の制御点Ｐａだけの移動では失敗の回避が難しい場合には、制御点Ｐａの追加が必要と判定する。

コンピュータ２０は、経路Ｒの評価及び最適化を行う（工程Ｓ８）。経路Ｒの評価には、下記式（１）に示す目的関数Ｌ_１を用いてよい。

式（１）において、ｇ_１は修正指示を反映した評価関数であり、ｇ_２は修正前の制御点からの乖離の度合いを評価する関数であり、ｇ_３は経路Ｒの長さを評価する関数であり、ｇ_４は互いに隣接する制御点Ｐａ同士の近さを評価する関数である。そして、α_１、α_２、α_３及びα_４は、ｇ_１、ｇ_２、ｇ_３、及びｇ_４のそれぞれに乗算される重みパラメータである。衝突情報である失敗点ｆの集合（Ｆ）、修正ベクトルｍの集合（Ｍ）及び修正パラメータβの集合（Ｂ）は、第１項に示す評価関数ｇ_１に反映させる。なお、図８は、評価関数ｇ_１の分布の例示である。障害物２００に近くなるほど、高いコストが設定され、障害物２００から離れるほど低いコストが設定されている。なお、このコスト分布は例示であるから、障害物２００の態様に応じて適宜設定してよい。

そして、式（２）に示すように、目的関数Ｌ_１が最小となる制御点列Ｘ_Ｂを算出する。

コンピュータ２０は、制御点列Ｘ_Ｂを満たすように、経路Ｒを修正する（工程Ｓ９）。そして、コンピュータ２０は、修正された経路Ｒを新たな基準経路として扱う。そして、コンピュータ２０は、出発点ＰＳからの追従制御を開始する（工程Ｓ３）。コンピュータ２０は、衝突なしに出発点ＰＳから最終目標点ＰＧに至る到達可能経路ＲＰが生成されるまで、工程Ｓ３、工程Ｓ４、工程Ｓ６、工程Ｓ７、工程Ｓ８、工程Ｓ９をこの順に行う動作を繰り返す。

＜修正パラメータβの調整＞
コンピュータ２０は、生成工程によって得た値を用いて、修正パラメータβを調整する（工程Ｓ１１）。修正パラメータβを調整した結果、調整された修正パラメータβ_ＮＥＷが得られる。具体的には、第１の環境で用いた修正パラメータβの集合Ｂ_０を用いて経路Ｒの自動生成を行った結果、失敗点ｆの集合Ｆ、修正ベクトルｍの集合Ｍ、到達可能経路ＲＰの最終制御点の集合Ｘ’を得た。そして、失敗点ｆの集合Ｆ、修正ベクトルｍの集合Ｍ、修正パラメータβの集合Ｂ_０を用いて、調整された修正パラメータβ_ＮＥＷを導出する。調整された修正パラメータβ_ＮＥＷを導出は、式（３）より示される最小化問題を解くことである。具体的には、修正回数Ｎにおける失敗点ｆの集合Ｆと、修正ベクトルｍの集合Ｍの履歴のうち、１からｋ（＝１、…Ｎ_Ｆ－１）までの履歴を示す集合Ｆ_ｋ及び集合Ｍ_ｋを利用して、調整された修正パラメータβ_ＮＥＷを適用した経路生成を行う。この経路の制御点集合は、下記式（３）によって定義される。Ｘ_Ｂ～Ｘ’を満たすように修正パラメータβの集合Ｂを調整した結果、調整された修正パラメータβ_ＮＥＷが得られる。

ここで、目的関数Ｌ_２は、式（４）によって定義される。

ここで、ｈ_１は、到達可能経路ＲＰに近づけることを目標とする関数である。また、ｈ_２は、Ｂ＝０となる意味のない解を排除するための関数である。

＜異なる環境での経路の自動生成への修正パラメータの適用＞
ところで、前述の経路の自動生成は、ある与えられた環境の基に実行される。しかし、経路の自動生成が行われる環境は、前回と今回とで同じ場合もあり得るし、同じでない場合もあり得る。本願の経路生成方法は、環境が異なる場合に有効に適用できる。つまり、本願の経路生成方法は、それまでに行った経路の自動生成の結果を利用した調整された修正パラメータβ_ＮＥＷを用いることによって、より少ない試行回数で、目標の経路Ｒを生成することができる。例えば、第１の環境における経路の自動生成の動作において、ｎ回の試行回数を要したとする。次に、第１の環境とは異なる第２の環境における経路の自動生成を行うとする。ここで、上述した修正パラメータβの調整を行うことなく、第１の環境の場合と同じ修正パラメータβを用いた経路の自動生成では、ｍ回の試行回数を要したとする。これに対して、上述した修正パラメータβの調整を行い、第１の環境の場合とは異なる調整された修正パラメータβ_ＮＥＷを用いた経路の自動生成では、ｍ回より少ない試行回数で経路を生成できる。

ここで、第１の環境での経路の自動生成の結果、調整された修正パラメータβ_ＮＥＷを、第１の環境とは異なる第２の環境での経路の自動生成にどのように適用するかという技術的な課題がある。この技術的な課題は、以下のような手法によって解決できる。

第１の環境と、第２の環境とは、全体として異なる。しかし、衝突が発生したときの微視的な環境を対比すると、類似するとみなせる場合がある。この微視的な類似は、衝突点に対する経路Ｒの方向と、反力の方向（修正ベクトルの方向）と、によって判断してよい。つまり、第２の環境で衝突が発生した点における経路Ｒの方向及び修正ベクトルｍの方向の関係が、第１の環境で衝突が発生した点における経路Ｒの方向及び修正ベクトルｍの方向の関係と同等であるとき、当該第１の環境の衝突点に対応する修正パラメータβを第２の環境における衝突点に対応する修正パラメータβとして適用できる。

例えば、コンピュータ２０は、経路評価及び最適化を行う工程Ｓ８において、以下のような動作を行うことにより、上述した第１の環境で得られた調整された修正パラメータβ_ＮＥＷを第２の環境での経路Ｒの自動生成に適用できる。

図６（ａ）に示すように、衝突が発生したと判定した後、コンピュータ２０は、経路Ｒの再設定に要する情報を取得する。経路Ｒの再設定に要する情報とは、接触力の方向を示す情報（衝突力方向情報Ｔ）と、衝突が生じた位置における経路Ｒの方向を示す情報（経路方向情報Ｅ）である。衝突力方向情報Ｔは、力覚センサ１４ａから受信した情報から得ることができる。衝突力方向情報Ｔは、経路Ｒを定義する座標系ＣＳに基づく角度φＴであってもよい。経路Ｒの方向を示す情報は、衝突が生じた位置を示す情報と、現在の経路Ｒを示す情報と、から得ることができる。衝突が生じた位置を示す情報は、ロータリエンコーダ１３ｂから受信した情報から算出することができる。そして、衝突が生じた位置がわかれば、経路Ｒ上のどこで衝突が生じたかを知ることができる。その結果、衝突が生じた経路Ｒ上の点における方向がわかる。経路Ｒ上の点における方向は、当該点における接線であってもよい。経路方向情報Ｅは、経路Ｒを定義する座標系ＣＳに基づく角度φＥであってもよい。

次に、コンピュータ２０は、衝突力方向情報Ｔ及び経路方向情報Ｅを利用して、修正パラメータβを得る。具体的には、図６（ｂ）に示すように、コンピュータ２０のＲＯＭ２０ｃには、衝突力方向情報Ｔ（ｎ）及び経路方向情報Ｅ（ｎ）との組み合わせと、当該組み合わせに対して紐付けされた修正パラメータβ（ｎ）と、を含むデータＤ（１）～Ｄ（Ｎ）がデータベースとして記憶されている。コンピュータ２０は、衝突が発生したことをきっかけにして得た衝突力方向情報Ｔ及び経路方向情報Ｅと、ＲＯＭ２０ｃに記憶された衝突力方向情報Ｔ及び経路方向情報Ｅとをマッチングさせる。次に、衝突が発生したことをきっかけにして得た衝突力方向情報Ｔ及び経路方向情報Ｅの組み合わせに対応する衝突力方向情報Ｔ（ｎ）及び経路方向情報（ｎ）の組み合わせに紐付けされた修正パラメータβ（ｎ）を読み出す。

なお、上記の説明では、第１の環境での到達可能経路ＲＰの生成の結果を利用して、調整された修正パラメータβ_ＮＥＷを得た。そして、調整された修正パラメータβ_ＮＥＷを利用して、第２の環境での到達可能経路ＲＰを生成した。その後、第２の環境での到達可能経路ＲＰを生成した結果を利用して、調整された修正パラメータβ_ＮＥＷをさらに調整してもよい。つまり、調整された修正パラメータβ_ＮＥＷは、到達可能経路ＲＰを生成するごとに更新されてもよい。修正パラメータβ_ＮＥＷは、ｊ回の到達可能経路ＲＰの生成の結果の元にｊ回の調整がなされた結果であってもよい。その結果、より少ない回数で到達可能経路ＲＰが設定できる望ましい修正パラメータβ_ＮＥＷに近づけることが可能である。

図７を参照しながら、経路生成方法を実行するコンピュータ２０の機能構成について説明する。コンピュータ２０は、マニピュレータ制御部２１と、情報取得部２２と、衝突判定部２３と、制御点判定部２４と、経路最適化部２５と、経路更新部２６と、記憶部２７と、到達判定部２８と、修正パラメータ調整部２９と、を有する。コンピュータ２０の各機能部は、プロセッサ２０ａ及びＲＡＭ２０ｂ等のハードウェア上にプログラムを読み込ませることにより、プロセッサ２０ａの制御のもとで、通信モジュール２０ｄ、及び入出力モジュール２０ｅ等を動作させるとともに、ＲＡＭ２０ｂにおける情報の読み出し及び書き込みを行うことで実現される。コンピュータ２０のプロセッサ２０ａは、このコンピュータプログラムを実行することによってコンピュータ２０を図７の各機能部として機能させ、経路生成方法に対応する処理を順次実行する。このコンピュータプログラムの実行に必要な各種情報、及び、このコンピュータプログラムの実行によって生成された各種情報は、全て、ＲＯＭ２０ｃ、ＲＡＭ２０ｂ等の内蔵メモリ、又は、ハードディスクドライブなどの記憶媒体に格納される。

マニピュレータ制御部２１は、工程Ｓ３に示す動作を行う。つまり、マニピュレータ制御部２１は、エンドエフェクタ１４を経路Ｒに沿って移動させる制御信号を出力する。情報取得部２２は、工程Ｓ６に示す動作を行う。つまり、情報取得部２２は、失敗点ｆの位置と、反力の方向（つまり、修正ベクトルｍの方向）と、を取得する。衝突判定部２３は、工程Ｓ４に示す動作を行う。つまり、衝突判定部２３は、衝突の有無を判定する。

制御点判定部２４は、工程Ｓ７に示す動作（判定工程）を行う。つまり、制御点判定部２４は、制御点Ｐａの追加の要否を判定する。また、制御点判定部２４は、制御点Ｐａの削除が可能であるかを判定してもよい。経路最適化部２５は、工程Ｓ８に示す動作を行う。つまり、経路最適化部２５は、式（１）に示す最適化問題を解く。経路更新部２６は、工程Ｓ９に示す動作を行う。つまり、経路更新部２６は、経路最適化部２５が出力する制御点列Ｘ_Ｂを満たすように、経路Ｒを修正する。

到達判定部２８は、工程Ｓ５に示す動作を行う。つまり、到達判定部２８は、エンドエフェクタ１４が最終目標点ＰＧへ到達したか否かを判定する。修正パラメータ調整部２９は、工程Ｓ１１に示す動作を行う。つまり、修正パラメータ調整部２９は、到達可能経路ＲＰの自動生成の動作によって得た値を用いて、修正パラメータβを調整して、調整された修正パラメータβ_ＮＥＷを得る。

＜作用効果＞
経路生成方法は、エンドエフェクタ１４が障害物２００に衝突することなく出発点ＰＳから最終目標点ＰＧにまで至る到達可能経路ＲＰを生成する。経路生成方法は、エンドエフェクタ１４が障害物２００に衝突したことを検知する工程Ｓ４と、衝突が検知された場合に、衝突が発生した位置に関する衝突位置情報と衝突によってエンドエフェクタ１４が受けた反力の向きに関する反力方向情報とを記録する工程Ｓ６と、修正ベクトルｍ及び修正パラメータβを利用して、衝突を生じさせないように経路Ｒを修正する工程Ｓ８、Ｓ９と、を含む。さらに、経路生成方法は、工程Ｓ４、Ｓ６～Ｓ９を繰り返して、到達可能経路ＲＰを生成する生成工程Ｓ２０と、工程Ｓ２０の後に、衝突位置情報及び反力方向情報を利用して、工程Ｓ２０において到達可能経路ＲＰが生成されるまでに要した繰り返し数よりも少ない繰り返し数で到達可能経路ＲＰが生成されるように、修正パラメータを調整する調整工程Ｓ１１と、を有する。

コンピュータ２０は、エンドエフェクタ１４が障害物２００に衝突することなく出発点ＰＳから最終目標点ＰＧにまで至る到達可能経路ＲＰを生成する。コンピュータ２０は、衝突が発生した位置に関する衝突位置情報及び衝突によって移動物体が受けた反力の向きに関する反力方向情報を受け入れる情報取得部２２と、衝突位置情報及び反力方向情報を利用して、到達可能経路ＲＰを生成する少なくとも１個のプロセッサ２０ａと、を備える。プロセッサ２０ａは、エンドエフェクタ１４が障害物２００に衝突したことを検知する動作と、衝突が検知された場合に、衝突が発生した位置に関する衝突位置情報と衝突によって移動物体が受けた反力の向きに関する反力方向情報とを記録する動作と、修正ベクトルｍ及び修正パラメータβを利用して、衝突を生じさせないように経路Ｒを修正する動作と、を実行する。プロセッサ２０ａは、これらの動作を繰り返して、到達可能経路ＲＰを生成し、到達可能経路ＲＰが生成された後に、衝突位置情報及び反力方向情報を利用して、到達可能経路ＲＰが生成されるまでに要した繰り返し数よりも少ない繰り返し数で到達可能経路ＲＰが生成されるように、修正パラメータβを調整することにより、調整された修正パラメータβ_ＮＥＷを得る。

経路生成方法及び経路生成装置によれば、到達可能経路ＲＰを生成するごとに、修正パラメータβが修正される。その結果、到達可能経路ＲＰを生成するごとに、到達可能経路ＲＰの生成に要する繰り返し数が少なくなるので、到達可能経路ＲＰの生成に要する時間を短縮することができる。

本発明は、上述した実施形態に限定されない。上記実施形態の構成は、様々に変更することができる。

１０…マニピュレータ、１１…ベース、１２…リンク、１３…ジョイント、１３ａ…モータ、１３ｂ…ロータリエンコーダ、１４…エンドエフェクタ（移動物体）、１４ａ…力覚センサ、２０…コンピュータ（経路生成装置）、２０ａ…プロセッサ、２１…マニピュレータ制御部、２２…情報取得部、２３…衝突判定部、２４…制御点判定部、２５…経路最適化部、２６…経路更新部、２７…記憶部、２８…到達判定部、２９…修正パラメータ調整部、１００…ロボットシステム、２００…障害物、ｍ…修正ベクトル、β…修正パラメータ、β_ＮＥＷ…調整された修正パラメータ。

Claims (6)

1. 移動物体が障害物に衝突することなく出発点から最終目標点にまで至る到達可能経路を生成する経路生成方法であって、
   前記移動物体が障害物に衝突したことを検知する工程と、前記衝突が検知された場合に、前記衝突が発生した位置に関する衝突位置情報と前記衝突によって前記移動物体が受けた反力の向きに関する反力方向情報とを記録する工程と、前記衝突位置情報、前記反力方向情報、及び前記反力の向きに向かって経路の一部を移動させる量を示す修正パラメータを利用して、前記衝突を生じさせないように前記経路を修正する工程と、を含み、前記検知する工程、前記記録する工程及び前記修正する工程を繰り返して、前記到達可能経路を生成する生成工程と、
   前記生成工程の後に、前記衝突位置情報及び前記反力方向情報を利用して、前記生成工程において前記到達可能経路が生成されるまでに要した繰り返し数よりも少ない繰り返し数で前記到達可能経路が生成されるように、前記修正パラメータを調整する調整工程と、を有する、経路生成方法。
2. 前記経路を修正する工程では、予め準備された前記修正パラメータを読み出す工程を含み、
   前記修正パラメータは、前記衝突が発生した位置における前記経路の方向であって前記衝突位置情報から導かれる経路方向情報及び前記反力方向情報の組み合わせに紐づけられて予めデータベースに記憶されている、請求項１に記載の経路生成方法。
3. 前記修正パラメータを読み出す工程では、前記記録する工程の結果として得られた前記衝突位置情報及び前記反力方向情報を用いて、前記経路方向情報及び前記反力方向情報の組み合わせを導出し、前記組み合わせと、前記データベースに記憶されている組み合わせとをマッチングさせることによって、前記修正パラメータを読み出す、請求項２に記載の経路生成方法。
4. 予め準備された前記修正パラメータは、第ｎの環境における第ｎの到達可能経路を生成した後に調整された第ｎの修正パラメータであり、前記第ｎの環境とは異なる第ｎ＋１の環境において第ｎ＋１の到達可能経路を生成するとき、前記第ｎの修正パラメータを用いると共に、前記第ｎ＋１の到達可能経路を生成したのちに前記第ｎの修正パラメータを調整して第ｎ＋１の修正パラメータを得る、請求項１～３の何れか一項に記載の経路生成方法。
5. 前記生成工程は、前記経路を規定する制御点の追加または削除の要否を判定する判定工程をさらに含む、請求項１～４のいずれか一項に記載の経路生成方法。
6. 移動物体が障害物に衝突することなく出発点から最終目標点にまで至る到達可能経路を生成する経路生成装置であって、
   前記衝突が発生した位置に関する衝突位置情報及び前記衝突によって前記移動物体が受けた反力の向きに関する反力方向情報を受け入れる情報取得部と、
   前記衝突位置情報及び前記反力方向情報を利用して、前記到達可能経路を生成する少なくとも１個のプロセッサと、を備え、
   前記少なくとも１個のプロセッサは、
   前記移動物体が障害物に衝突したことを検知する動作と、前記衝突が検知された場合に、前記衝突が発生した位置に関する前記衝突位置情報と前記衝突によって前記移動物体が受けた反力の向きに関する前記反力方向情報とを記録する動作と、前記衝突位置情報、前記反力方向情報、及び前記反力の向きに向かって経路の一部を移動させる量を示す修正パラメータを利用して、前記衝突を生じさせないように前記経路を修正する動作と、を含んで前記検知する動作、前記記録する動作及び前記修正する動作を繰り返して、前記到達可能経路を生成し、
   前記到達可能経路を生成した後に、前記衝突位置情報及び前記反力方向情報を利用して、前記到達可能経路が生成されるまでに要した繰り返し数よりも少ない繰り返し数で前記到達可能経路が生成されるように、前記修正パラメータを調整する、経路生成装置。
   

Images