移動体軌道生成方法および移動体軌道生成装置
【課題】移動体の動作軌道の自動生成方法として従来ポテンシャル法が用いられていたが、ローカルミニマムによって軌道生成不能となることがあった。
【解決手段】作業空間に障害物と移動体の初期位置と目標位置とを結ぶ動作軌道モデルを設定する。さらに、作業空間中に障害物を発生源とする斥力ベクトル場を設定する。この斥力ベクトル場を、動作軌道モデルを構成する質点に作用させて動作軌道モデルを変形させることにより、移動体の動作軌道を生成する。
【解決手段】作業空間に障害物と移動体の初期位置と目標位置とを結ぶ動作軌道モデルを設定する。さらに、作業空間中に障害物を発生源とする斥力ベクトル場を設定する。この斥力ベクトル場を、動作軌道モデルを構成する質点に作用させて動作軌道モデルを変形させることにより、移動体の動作軌道を生成する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、障害物の存在する環境で移動する物体の最適な軌道を生成する移動体軌道生成方法および移動体軌道生成装置に係る。
【背景技術】
【０００２】
従来、ロボットの動作設計にはオフラインティーチングによる動作記録方式若しくは制御プログラムによる方式がある。動作記録方式は組立作業や溶接作業などあらかじめ決められた動作を行う産業用ロボットに広く用いられており、実際の作業環境に合わせた動作の設計が可能であるという利点がある。その一方で、ティーチングに多くの時間がかかると共に、動作の設計に熟練を要するといった欠点がある。制御プログラムによる方式はセンサ情報を用いて判断を行い、ある程度環境にロバストな動作設計を行なうことが可能である。しかし、干渉を回避するための軌道の生成を、シミュレーション等を利用して確認しながら試行錯誤的に行う必要があり、設計と検証に多大な労力が必要となる。
【０００３】
これに対して、ロボットの動作設計の自動化の試みとしてはポテンシャル場を用いた経路探索手法（例えば、特許文献１参照。）がある。この手法では作業空間内に目標位置が最小、現在位置が最大となるような仮想的なポテンシャルを設定する。さらに、障害物にも大きなポテンシャルを持たせることで、ポテンシャルが低くなる方向へ軌道を生成することで、障害物を回避しながら目標位置までの経路を生成できる。
【特許文献１】特開２００３−２８０７１０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
上述したポテンシャル場を用いた経路探索手法では、ポテンシャル場にローカルミニマムが生じて、経路探索時にローカルミニマムに陥ると軌道を生成できないという問題があった。ローカルミニマムとは、ポテンシャルが局所的に極小となる領域を指す。経路探索時にローカルミニマムの領域に入ると、周囲のポテンシャルがローカルミニマムの領域より高く、経路探索を続行できずに軌道生成不能となるのである。
【０００５】
複雑形状の障害物がある環境では、ローカルミニマムの存在しないポテンシャル場を設定することは非常に困難である。また、ローカルミニマムを回避するために迂回した経路を生成するとなると、軌道が長大になり、不要な変曲点を含む軌道となるなど、最適な軌道とならず、軌道の最適化を図る必要等があった。
【０００６】
上述した課題に鑑み、本発明の目的は、ロボットマニピュレータの手先等の目標軌道を試行錯誤的に行うことなく生成でき、かつ、上述したローカルミニマムの問題が生じない移動体軌道生成方法および移動体軌道生成装置を提供することにある。
【課題を解決するための手段】
【０００７】
上記目的を達成するため、本発明による移動体軌道生成方法は、移動体が移動する空間を設定する空間設定ステップと、前記空間中に前記移動体が動作を開始する初期位置を設定する初期位置設定ステップと、前記空間中に前記移動体の目標位置を設定する目標位置設定ステップと、前記空間中に障害物の位置を設定する障害物設定ステップと、前記初期位置と前記目標位置とを結び、前記初期位置と前記目標位置の間に質点を有し、前記初期位置と前記目標位置に両端を固定された動作軌道モデルを設定する動作軌道モデル設定ステップと、前記空間に前記障害物を発生源とする斥力ベクトルからなる斥力ベクトル場を設定する斥力ベクトル場設定ステップと、前記斥力ベクトル場の前記斥力ベクトルを前記質点に作用させ、変形した前記動作軌道モデルを前記移動体の動作軌道とする動作軌道生成ステップと、を備えることを特徴とする。
【０００８】
また、本発明による移動体軌道生成装置は、移動体が移動する空間を設定する空間設定手段と、前記空間中に前記移動体が動作を開始する初期位置を設定する初期位置設定手段と、前記空間中に前記移動体の目標位置を設定する目標位置設定手段と、前記空間中に障害物の位置を設定する障害物設定手段と、前記初期位置と前記目標位置とを結び、前記初期位置と前記目標位置の間に質点を有し、前記初期位置と前記目標位置に両端を固定された動作軌道モデルを設定する動作軌道モデル設定手段と、前記空間に前記障害物を発生源とする斥力ベクトルからなる斥力ベクトル場を設定する斥力ベクトル場設定手段と、前記斥力ベクトル場の前記斥力ベクトルを前記質点に作用させ、変形した前記動作軌道モデルを前記移動体の動作軌道とする動作軌道生成手段と、を備えることを特徴とする。
【発明の効果】
【０００９】
本発明の移動体軌道生成方法および移動体軌道生成装置によれば、従来のポテンシャル法による軌道生成に比較して、ローカルミニマムの問題が生ずることなく移動体の最適な目標軌道を生成することができる。
【発明を実施するための最良の形態】
【００１０】
以下本発明の実施例について図面を参照しながら説明する。
【実施例１】
【００１１】
本発明の第１の実施例について、図面を用いて以下説明する。図１は、本実施例による移動体軌道生成方法の主要なステップを記載したフローチャートである。本実施例の移動体軌道生成方法において行われる処理について、図１を用いて説明する。なお、各実施例においては、軌道が生成される移動体はロボットマニピュレータの手先であるものとして以下説明する。
【００１２】
まず、ロボットマニピュレータが作業を行う作業空間を定義する処理を行う（ステップＳ１）。作業空間とは、ロボットマニピュレータの初期位置や作業位置、および初期位置と作業位置の中間を含む実際の空間の情報に基づいて構築される空間である。この作業空間は、作業を行う施設等の設計情報、例えば３次元ＣＡＤのデータを用いて設定する、またはカメラの映像や各種センサによる計測等で収集したデータを用いて設定する、あるいは手動で設定する、といったことが考えられる。
【００１３】
具体的には、例えばロボットマニュピレータの操作装置が関連付けられ、各種３次元形状データを作成、記憶、修正可能な３次元ＣＡＤシステムを構成する演算装置及び入力装置によって行われる。すなわち、上述の入力装置に施設等の設計情報や収集したデータを入力する、あるいは任意に施設情報等を入力して、上述した作業空間に対応する３次元形状データを設定することによって行われる。
【００１４】
次に、定義された作業空間内に存在する障害物の位置と形状を定義する処理を行う（ステップＳ２）。この障害物の定義も、作業空間の定義と同様に、設計情報やデータ収集、手動設定等によって行うことができる。また、障害物が作業空間中を移動する場合は、障害物の移動情報を反映させて移動する障害物を設定することもできる。
【００１５】
次に、作業空間上に斥力ベクトル場を生成する処理を行う（ステップＳ３）。斥力ベクトル場の生成は、作業空間上に３次元の格子を設定し、各格子点に斥力ベクトルを定義することで行う。この斥力ベクトルはステップＳ２で定義された障害物を発生源として設定する。各格子点に設定される斥力ベクトルの方向は障害物表面の法線方向、すなわち障害物表面に垂直な方向とし、斥力ベクトルの大きさは障害物に近いほど大きくなるように設定する。作業空間上に複数の障害物が存在する場合、各格子点に各障害物から発生する斥力ベクトルを合算したものが、最終的にその格子点に設定される斥力ベクトルとなる。
【００１６】
ステップＳ２で定義された障害物と、ステップＳ３で生成された斥力ベクトル場の例を図２に示す。図２は作業空間に設定された障害物と斥力ベクトル場の概要を示す概念図である。なお、斥力ベクトル場については、斥力ベクトルのおおよその方向のみ示しており、作業空間上に設定される３次元格子は省略している。作業空間中に円筒形状の障害物１０が設定されており、この障害物１０から遠ざかる方向の斥力ベクトル１１が設定されている。
【００１７】
斥力ベクトル場を設定する際、障害物１０が複雑な形状で、突起や極端な狭隘部を有する等のロボットマニピュレータの手先が通過しにくい形状や配置を取る場合は、該当箇所付近の斥力ベクトル１１を大きくする等の重み付けを行ってもよい。
【００１８】
次に、移動体の初期位置と目標位置を結ぶように、動作軌道モデルを作業空間上に生成・配置する処理を行う（ステップＳ４）。この動作軌道モデルの構造の一例を、図３を用いて説明する。動作軌道モデルは、引っ張りバネ２０とヒンジバネ２１を組み合わせたバネマス２２を複数結合し、バネマス２２同士の結合部には質点２３が設定された連成バネマス構造を有する。また、動作軌道モデルの両端に設定される質点２３ａ、質点２３ｂは、動作軌道モデルの配置の際、質点２３ａは移動体の初期位置に、質点２３ｂは移動体の目標位置に固定される。なお、図３ではバネマス２２を２つ結合したものとして図示しているが、バネマス２２の数は任意に設定することができる。
【００１９】
なお、図３においては、移動体の初期位置と目標位置とを直線的に結ぶように動作軌道モデルを配置しているが、直線的に結ぶ必要はなく、空間中で初期位置と目標位置とを任意に結ぶように配置されていればよい。
【００２０】
次に、ステップＳ４で生成した動作軌道モデルにステップＳ３で設定した斥力ベクトル１１を作用させ、動作軌道を生成する処理を行う（ステップＳ５）。動作軌道モデルに設定された各質点２３について、各質点２３周辺に設定された複数の斥力ベクトル１１の平均となるベクトルを作用させる。すると、動作軌道モデルを構成する引っ張りバネ２０、ヒンジバネ２１が変形する。この際、ヒンジバネ２１は隣接する質点２３間を直線形状に保つ方向に力を作用させ、ロボットマニピュレータの動作軌道３１が必要以上に湾曲することを防ぐ。
【００２１】
最終的には、各質点２３に作用する斥力ベクトル１１と引っ張りバネ２０、ヒンジバネ２１の張力がつりあう形状に動作軌道モデルが変形する。この変形した動作軌道モデルの描く軌道がロボットマニピュレータの動作軌道となる。なお、質点２３周辺に斥力ベクトル１１を作用させる際、斥力ベクトル１１が設定された各格子点と斥力ベクトル１１の質点２３との距離に基づいて、複数の斥力ベクトル１１に対して重み付けを行ってベクトルを計算することもできる。
【００２２】
ステップＳ５における動作軌道の生成の例を、図４を用いて説明する。図４（ａ）は動作軌道モデルに斥力ベクトル１１を作用させていない状態、図４（ｂ）は動作軌道モデルに斥力ベクトル１１を作用させて動作軌道を生成した状態を示している。なお、図４においては簡略的に２次元図で表し、各質点２３を結ぶバネマス２２も簡略化して図示している。
【００２３】
図４（ａ）で示すようにステップＳ４において動作軌道モデルが障害物１０を貫通するように配置される。なお、上述したように、質点２３ａは移動体の初期位置に、質点２３ｂは移動体の目標位置に固定されている。この動作軌道モデルに斥力ベクトル１１を作用させると、図４（ｂ）で示す形状に変形し、障害物１０を紙面右側から迂回する動作軌道が生成される。質点２３ａ、質点２３ｂは位置が固定されているため移動しない。
【００２４】
次に、ステップＳ５で生成した動作軌道について、動作軌道モデルの各質点２３間を曲線補間し、動作軌道を滑らかにする処理を行う（ステップＳ６）。
【００２５】
以上の一連の処理により、ロボットマニピュレータの手先の動作軌道が生成される。このような動作軌道生成によれば、ポテンシャル法におけるローカルミニマムの問題が生じることなく、移動体の最適な動作軌道を生成することが可能である。また、動作軌道モデルに連成バネマス構造を採用し、各バネのバネ定数を任意に設定可能とすることで、障害物の配置や形状に応じ、障害物に対する移動体の回避軌道の大きさを任意に変更することが可能となる。
【００２６】
また、ステップＳ１について説明したとおり移動する障害物を設定する場合は、リアルタイムに斥力ベクトル場と動作軌道モデルの形状を更新する処理を行うことも可能である。また、障害物の移動範囲が決まっている場合は、障害物の移動範囲全体を一つの障害物として設定することも可能である。
【００２７】
また、本発明の移動体動作軌道生成方法は、設計情報や、実際の作業環境の情報を収集して設定することができるため、設計・計画段階の施設、建設済みの施設の何れについても適用することが可能である。
【００２８】
また、動作軌道モデルを構成する引っ張りバネ２０、ヒンジバネ２１のバネ定数は任意に設定することができる。斥力ベクトル場が同一の場合、バネ定数が大きいほど動作軌道モデルは変形し難く、逆にバネ定数が小さいほど動作軌道モデルは変形し易くなる。したがって、例えば、手先が大きい場合はバネ定数を小さくすることにより、バネ定数が固定の場合の動作軌道よりも障害物１０から離れた動作軌道が生成される。逆に、手先が小さい場合はバネ定数を大きくすることでバネ定数が固定の場合の動作軌道よりも障害物１０に近い動作軌道が生成される。換言すれば、バネ定数を任意に設定することにより、障害物１０に対する移動体の回避軌道の大きさを任意に変更することが可能である。
【００２９】
つまり、上述の方法により手先の大きさに合わせてロボットマニピュレータと障害物１０の干渉回避距離を確保した動作軌道を生成することができる。また、動作軌道モデルを構成する各バネのバネ定数は同一でなくともよい。例えば、障害物１０が少ない箇所におけるバネ定数を大きくし、障害物１０が多く狭隘な箇所におけるバネ定数を小さくするなど、作業空間内１の障害物１０の形状に合わせて部分的にバネ定数を変えることで、バネ定数が大きい箇所では障害物１０の形状に依存することなく、より直線的な軌道を得ることができ、バネ定数が小さい箇所では障害物１０の形状に沿った干渉回避軌道を得ることができる。
【００３０】
また、動作軌道を生成する際に、質点２３の各々についてロボットマニピュレータの可動範囲を拘束条件としてもよい。すなわち、質点２３の各々は斥力ベクトル１１を作用させることで作業空間中を移動するが、この際にロボットマニピュレータの可動範囲外には移動できないよう設定することで、確実にロボットマニピュレータの可動範囲内に動作軌道を生成することが可能である。
【００３１】
また、本実施例においては動作軌道モデルに関して、間隔を空けて質点２３を離散的に配置したものとして説明したが、連続的に質点２３を配置したものとして、つまり動作軌道モデルが全体として質量を有するものとすることも可能である。
【実施例２】
【００３２】
本発明の第２の実施例について、図面を用いて以下説明する。なお、第１の実施例と同じ構成には同一の符号を付し、重複する説明は省略する。
【００３３】
本実施例においては、ステップＳ４で生成される動作軌道モデルに、図５に示す連成バネマスダンパ構造を採用している。この連成バネマスダンパ構造は、連成バネマス構造の質点２３と引っ張りバネ２０の間にダンパ２４を追加した構造である。
【００３４】
この連成バネマスダンパ構造を採用することにより、ステップＳ５で動作軌道モデルに斥力ベクトル１１を作用させて軌道を生成する際に、動作軌道モデル変形時の各質点２３間の振動、つまり引っ張りバネ２０の伸縮方向の振動を抑制することができる。
【００３５】
また、各ダンパ２４の減衰係数を可変とし、動的に位置が変動する障害物１０等により瞬間的に過大な斥力が作用した場合にダンパ２４の減衰係数が増加する設定、すなわちダンパ２４に印加される応力の増加に応じて減衰係数が増加するよう設定することで、動作軌道が部分的に大きく変形することを防止することができる。
【００３６】
また、各ダンパの減衰係数を個別に設定可能とし、たとえば移動体の初期位置と目標位置付近のダンパは減衰係数を大きくして動作軌道の変形を抑制し、他のダンパは減衰係数を小さくして動作軌道を変形しやすくする等、動作軌道モデルのバネ定数を個別に異なる設定とした場合と類似した効果を得ることができる。
【００３７】
なお、当然ながら各質点２３間の全箇所にダンパ２４を設定する必要はなく、任意の箇所にのみダンパを追加した連成バネマスダンパ構造とすることも可能である。また、質点２３と引っ張りバネ２０の間にダンパ２４を追加したものとして説明したが、当然ヒンジバネ２１と質点２３の間、あるいは引っ張りバネ２０とヒンジバネ２１の間にダンパ２４を追加することも可能である。
【実施例３】
【００３８】
本発明の第３の実施例について以下説明する。
【００３９】
本実施例の移動体動作軌道生成方法では、動作軌道モデルを生成するステップＳ４において、各質点２３に粘性摩擦係数を設定する。ここでいう各質点２３の粘性摩擦とは、作業空間上を質点２３が移動する際に、質点２３の移動に対して抵抗力として働くものである。すなわち、この粘性摩擦係数が高いほど質点２３は移動しにくくなる。この粘性摩擦係数を設定することにより、動作軌道モデルに斥力ベクトル１１を作用させて動作軌道を生成する際の動作軌道モデルの振動を抑制することが可能である。質点２３の粘性摩擦は全方向の移動に対して作用するため、連成バネマスダンパ構造のダンパと異なり、動作軌道モデル全体のあらゆる方向の振動を抑制することが可能である。
【００４０】
この粘性摩擦係数は、第２の実施例で説明したダンパの減衰係数と同様に、質点２３に働く斥力の増加にともなって増加する設定とすることができ、また、各質点２３について個別に設定することが可能である。
【実施例４】
【００４１】
本発明の第４の実施例について以下説明する。
【００４２】
本実施例においては、動作軌道モデルを生成するステップＳ４において、生成される動作軌道モデルを構成するバネの自然長を個別に設定し、各質点２３間の距離を個別に設定可能とするものである。また、ステップＳ５で生成された動作軌道について、移動体の動作速度を各質点２３間の距離と関連付けることができ、例えば生成された動作軌道について、動作軌道モデルの各質点２３間の移動に要する時間を一定とした場合、質点２３間の距離が長い場合は移動体の動作速度が速く、質点２３間の距離が短い場合は移動体の動作速度を遅くするよう設定することができる。
【００４３】
さらに、上述した実施例と組み合わせ、例えばバネ定数の個別設定によって障害物１０に対して大きく回避軌道をとる設定とした箇所では移動体の動作速度を速く、障害物１０に対して小さく回避軌道を取る設定とした箇所では移動体の動作速度を遅くする、といった設定が可能である。
【００４４】
本実施例によれば、動作軌道の生成時に、移動体の動作速度も自動で最適な設定とすることが可能である。
【実施例５】
【００４５】
本発明の第５の実施例について、図面を用いて以下説明する。
【００４６】
本実施例においては、軌道生成ステップＳ５にて動作軌道を生成する際に、動作軌道モデルの任意の質点２３を微小移動させる質点移動処理を行うことができる。
【００４７】
作業空間上に設定された障害物１０、移動体の初期位置、目標位置によっては、動作軌道モデルの各質点２３に作用する斥力ベクトルが釣り合ってしまい、ステップＳ５にて動作軌道モデルの各質点２３に斥力を作用させても動作軌道モデルが変形せず、障害物１０を回避する軌道が生成されない状況が考えられる。
【００４８】
このような状況の一例を図６に示す。図６では、動作軌道モデルの各質点２３に対して、紙面右方向に作用する斥力１１ａと紙面左方向に作用する斥力１１ｂが釣り合った状態を示している。図６では２次元的に示しているが、３次元の作業空間上で各質点２３に作用する斥力ベクトルが釣り合ってしまうと、図６のように動作軌道モデルが障害物１０を貫通する状態で維持されることになる。
【００４９】
そこで、本実施例においては、上述したように軌道生成ステップＳ５において、動作軌道モデルの各質点２３に作用する斥力ベクトルが釣り合った場合に、任意の質点２３を微小移動させる質点移動処理を行う。この質点移動処理により、質点２３に作用する斥力の均衡を崩し、ステップＳ５において正常に動作軌道を生成することが可能である。
【実施例６】
【００５０】
本発明の第６の実施例について、図面を用いて以下説明する。図７は実施例５の別の形態の概念図であり、作業空間中に引力源３０が設定されている。なお、破線は変形前の動作軌道モデルの配置を示している。
【００５１】
図７に示した形態では、ステップＳ５において作業空間の任意の位置に引力源３０を設定する処理を行う。引力源３０は、図７に示すように、各質点２３に対して引力源３０の方向へ働く引力ベクトル３１を与える。各質点２３に引力ベクトル３１を作用させることにより、生成される動作軌道について、引力源３０側の軌道を描くように制御することが可能である。例えば、ロボットマニピュレータ本体の設置位置に引力源を設定することにより、動作軌道をロボットマニピュレータに近い側に生成させることができる。あるいは、実際の作業空間に設置されたカメラに対応する位置に引力源３０を設定することにより、ロボットマニピュレータを監視し易い動作軌道とすることができる。
【００５２】
また、引力源３０を設定することにより、第５の実施例で説明したような斥力ベクトルの均衡によって動作軌道が正常に生成されない状況を解消することが可能である。
【実施例７】
【００５３】
本発明の第７の実施例について、図面を用いて以下説明する。なお、第１の実施例と同じ構成には同一の符号を付し、重複する説明は省略する。
【００５４】
図８は、本実施例による移動体軌道生成方法の主要なステップを記載したフローチャートである。本実施例による移動体軌道生成方法は障害物が移動する作業環境を想定したもので、動作軌道モデルを生成・配置した後の処理が第１の実施例と異なっている。
【００５５】
作業空間の定義（ステップＳ１）から動作軌道モデルの生成・配置（ステップＳ４）までは第１の実施例と同様の処理を行う。
【００５６】
次に、軌道生成（ステップＳ１５）、曲線補間（ステップＳ１６）を行う。これらの処理も、この段階では第１の実施例の軌道生成（ステップＳ５）、曲線補間（ステップＳ６）と同様の処理である。
【００５７】
次に、作業空間に配置された障害物１０を移動する処理を行う（ステップＳ１７）。
【００５８】
次に、障害物１０の移動に伴う斥力ベクトル場の更新を行う。また、斥力ベクトル場の更新に合わせて動作軌道モデルに作用する斥力ベクトルを更新し、動作軌道モデルを更新する処理を行う（ステップＳ１８）。
【００５９】
次に、ステップＳ１８で更新された動作軌道モデルについて、動作軌道モデルを構成する各バネの張力が所定の閾値を超えているかいないかを判定する処理を行う（ステップＳ１９）。
【００６０】
ステップＳ１９の判定で、バネ張力が閾値より低かった場合は、移動体の移動を行う（ステップＳ２０）。そして、移動体が目標位置に到達したかどうか判定を行い（ステップＳ２１）、到達していた場合は処理を終了し、到達していない場合はステップＳ１７に戻り、障害物１０の移動以降のステップを繰り返し行う。
【００６１】
ステップＳ１９の判定で、バネ張力が閾値よりも高かった場合は、動作軌道モデルを再配置する処理を行う（ステップＳ２２）。この動作軌道モデルの再配置は、更新後の斥力ベクトル場の他、移動体の現在位置を考慮する点でステップＳ１４と異なる。ステップＳ１４では移動体の初期位置と目標位置のみを動作軌道モデルの拘束点として配置しているが、ステップＳ２２では移動体の初期位置、目標位置に加えて移動体の現在位置を拘束点として動作軌道モデルを再配置する。この動作軌道モデルの再配置後は、再度軌道生成（ステップＳ１５）、曲線補間（ステップＳ１６）を行い、障害物１０の移動以降のステップを繰り返す。
【００６２】
このような処理とすることにより、障害物１０の移動にともなって動作軌道を逐次更新することが可能である。また、動作軌道モデルを構成するバネの張力を判定することにより、障害物１０の移動によって更新された動作軌道モデルが極端な軌道を描く場合等に、動作軌道モデルの再設定を行うことによって、移動体の現在位置を考慮した最適な動作軌道への変更を自動で行うことが可能である。
【００６３】
本実施例による動作軌道の変更処理の例を、図面を用いて説明する。図９、図１０、図１１はそれぞれ、本実施例によるステップＳ１７以降の処理を説明するための概念図であり、図９は障害物１０が移動する前の状態、図１０は障害物１０が移動した後の状態、図１１はステップＳ２２を経て動作軌道を変更した例を示している。
【００６４】
図９はステップＳ１５で最初に動作軌道を生成した状態を示している。障害物１０が複雑な形状をしており、動作軌道モデルがこの障害物１０の凹部を通過するように迂回する軌道をとっている。矢印４０はこの障害物１０の移動方向を示し、破線は移動体の初期位置と目標位置とを直線的に結んだもので、変形前の動作軌道モデルの位置に相当する。
【００６５】
図１０はステップＳ１７によって障害物１０が移動し、ステップＳ１８で斥力ベクトル場と動作軌道とが更新された状態を示している。図９の状態と比較すると、障害物１０が矢印４０方向へ移動したことにより、動作軌道モデルが障害物１０をより大きく迂回するような軌道をとっていることが分かる。
【００６６】
図１１はステップＳ１９で動作軌道モデルのバネ張力が閾値より高いと判定され、ステップＳ２２の動作軌道モデルの再配置を経てステップＳ１５で動作軌道が変更された状態を示している。なお、図１１ではステップＳ２２の時点で移動体が質点２３ｃの位置まで移動していたものとして図示している。よって、移動体の初期位置、目標位置のほか、質点２３ｃの位置も動作軌道モデルの拘束点となっており、動作軌道が変更された結果、図９や図１０とは逆側から障害物１０を迂回するような軌道をとっている。なお、図１１においては、破線はステップＳ１９で判定された際の動作軌道モデルの形状を示している。
【００６７】
なお、本実施例によってはステップＳ１９において動作軌道モデルを構成するバネの張力に基づいて動作軌道モデルを再配置する処理の実行を判定するものとしたが、例えば動作軌道モデルがロボットマニピュレータの可動範囲から逸脱した場合に動作軌道モデルを再配置するような判定とすることも可能である。
【実施例８】
【００６８】
本発明の第８の実施例について、図面を用いて以下説明する。なお、第１の実施例と同じ構成には同一の符号を付し、重複する説明は省略する。
【００６９】
図１２は、本実施例による移動体軌道生成方法の主要なステップを記載したフローチャートである。本実施例による移動体軌道生成方法は、移動体であるロボットマニピュレータの手先の移動中におけるロボットマニピュレータと障害物との干渉を想定したもので、動作軌道モデルを生成した後の処理が第１の実施例と異なっている。
【００７０】
作業空間の定義（ステップＳ１）から動作軌道モデルの生成・配置（ステップＳ４）までは第１の実施例と同様の処理を行う。
【００７１】
次に、軌道生成（ステップＳ２５）、曲線補間（ステップＳ２６）を行う。これらの処理も、この段階では第１の実施例の軌道生成（ステップＳ５）、曲線補間（ステップＳ６）と同様の処理である。ただし、ロボットマニピュレータの設置位置が移動体の初期位置と異なる場合は、別途にロボットマニピュレータの設置位置を設定する処理を行う。
【００７２】
次に、移動体であるロボットマニピュレータの手先軌道から逆運動学を解き、ロボットマニピュレータの関節角度を算出し、ロボットマニピュレータの姿勢を計算する処理を行う（ステップＳ２７）。この計算には、ロボットマニピュレータの設置位置、手先の位置、ロボットマニピュレータの関節数や大きさ、形状等の情報を用いて行う。この際、手先が動作軌道の移動体初期位置から目標位置にかけて移動する間の任意の位置または全位置についてロボットマニピュレータ姿勢計算を行う。
【００７３】
次に、ステップＳ２７で計算したロボットマニピュレータの姿勢が障害物１０と干渉するかどうか判定する処理を行う（ステップＳ２８）。干渉すると判定された場合、ロボットマニピュレータが干渉する際の手先位置に近い質点２３に作用する斥力ベクトル１１をより大きな値に再設定する処理を行う（ステップＳ２９）。斥力ベクトル１１の再設定後、ステップＳ２５で動作軌道を再生成し、以降の処理を繰り返す。ステップＳ２８でロボットマニピュレータと障害物１０が干渉しないと判定された場合は、動作軌道の生成を終了する。
【００７４】
以下、本実施例による移動体軌道生成方法の効果について説明する。
【００７５】
各実施例で説明した移動体軌道生成方法によれば、障害物１０と干渉することがなく最適な移動体の動作軌道を生成することができる。しかし、移動体がロボットマニピュレータの手先である場合では、手先が障害物１０を回避する軌道であったとしても、ロボットマニピュレータの腕が障害物１０と干渉することが考えられる。そこで、手先が移動する間のロボットマニピュレータ自身の姿勢を逆運動学によって算出し、障害物１０とロボットマニピュレータとの干渉を回避する動作軌道を生成することが可能である。
【実施例９】
【００７６】
本発明の第９の実施例について、図面を用いて以下説明する。なお、第１の実施例と同じ構成には同一の符号を付し、重複する説明は省略する。
【００７７】
図１３は、本実施例による移動体軌道生成方法の主要なステップを記載したフローチャートである。本実施例による移動体軌道生成方法は、移動体であるロボットマニピュレータの手先の移動中におけるロボットマニピュレータと障害物との干渉を想定したもので、動作軌道モデルを生成した後の処理が第１の実施例と異なっている。
【００７８】
作業空間の定義（ステップＳ１）から動作軌道モデルの生成・配置（ステップＳ４）までは第１の実施例と同様の処理を行う。
【００７９】
次に、軌道生成（ステップＳ３５）、曲線補間（ステップＳ３６）を行う。これらの処理も、この段階では第１の実施例の軌道生成（ステップＳ５）、曲線補間（ステップＳ６）と同様の処理である。
【００８０】
次に、剛体リンク機構モデルを生成・配置する処理を行う（ステップＳ３７）。この剛体リンク機構モデルとはロボットマニピュレータをモデル化したもので、実際に使用するロボットマニピュレータと同数の関節を有し、関節と関節とを剛体のリンクで連絡した仮想的なモデルである。この剛体リンク機構モデルについては、動作軌道モデルの質点２３と同様に、関節とリンクの双方に斥力ベクトル場の斥力ベクトル１１が作用する。
【００８１】
次に、ロボットマニピュレータの姿勢を、剛体リンク機構と剛体リンク機構に作用する斥力ベクトル１１とを用いて計算する処理を行う（ステップＳ３８）。ロボットマニピュレータの姿勢計算は、第８の実施例のステップＳ２７と同様に、手先が動作軌道の移動体初期位置から目標位置にかけて移動する間の任意の位置または全位置についてロボットマニピュレータ姿勢計算を行う。
【００８２】
次に、ステップＳ３８で計算した剛体リンク機構の形状に基づき、手先が移動後位置に移動した際にロボットマニピュレータと障害物１０とが干渉するかを判定する処理を行う（ステップＳ３９）。干渉する場合は第８の実施例のステップＳ２９と同様に、斥力ベクトル１１を再設定し（ステップＳ４０）、ステップＳ３５で動作軌道を再生成して以降の処理を繰り返す。
【００８３】
ステップＳ３９でロボットマニピュレータと障害物１０が干渉しないと判定された場合は、動作軌道の生成を終了する。
【００８４】
本実施例によれば、第８の実施例で説明した移動体軌道生成方法と同様に、障害物１０とロボットマニピュレータとの干渉を回避する動作軌道を生成することが可能である。また、本実施例によれば、剛体リンク機構を採用したことにより、手先位置と剛体リンク機構に作用する斥力ベクトルからロボットマニピュレータの姿勢を計算することが可能である。したがって、ロボットマニピュレータの自由度が冗長で、逆運動学の解が一意に導き出せないような場合にも適用可能である。
【実施例１０】
【００８５】
本発明の第１０の実施例について、図面を用いて以下説明する。なお、第８の実施例と同じ構成には同一の符号を付し、重複する説明は省略する。
【００８６】
図１４は、本実施例による移動体軌道生成方法の主要なステップを記載したフローチャートである。本実施例による移動体軌道生成方法は、第８の実施例と同様に、移動体であるロボットマニピュレータの手先の移動中におけるロボットマニピュレータと障害物との干渉を想定したもので、障害物１０とロボットマニピュレータの干渉を判定した後の処理が第８の実施例と異なっている。
【００８７】
作業空間の定義（ステップＳ１）から、障害物１０とロボットマニピュレータの干渉の判定（ステップＳ２８）までは第８の実施例と同様の処理を行う。
【００８８】
次に、ステップＳ２８で干渉すると判定された場合に、斥力ベクトル１１を再設定するステップＳ２９の前に、斥力ベクトル１１の再設定回数の累積が閾値以上かを判定する処理を行う（ステップＳ５０）。
【００８９】
ステップＳ５０で、斥力ベクトル１１の再設定回数の累積が閾値未満の場合、ステップＳ２９に進み、斥力ベクトル１１の再設定を行う。斥力ベクトル１１の再設定回数の累積が閾値以上の場合、ロボットマニピュレータの設置位置を任意の位置に変更して再設定する処理を行う（ステップＳ５１）。この際、斥力ベクトル１１の再設定回数はリセットする。ロボットマニピュレータの設置位置を変更した後、ステップＳ４以降の処理を繰り返す。
【００９０】
本実施例の効果について、以下説明する。
【００９１】
第８の実施例について、斥力ベクトル１１を再設定してもロボットマニピュレータと障害物１０との干渉が解消されない場合、斥力ベクトル１１の再設定を繰り返し行うことになる。この場合、斥力ベクトル１１の再設定を繰り返しても干渉が回避されず処理が無限ループに陥る場合や、干渉が解消されたとしても動作軌道が極端に大きな回避軌道をとってしまう場合等が考えられる。
【００９２】
本実施例では、斥力ベクトル１１の再設定回数に累積閾値を設定して、閾値に達するまでの再設定で干渉が解消されない場合に、ロボットマニピュレータの初期位置変更を行う。これによって、上述したような無限ループや、極端な動作軌道が生成されることを防止することが可能である。
【００９３】
本実施例はロボットマニピュレータの設置位置を変更するものであるため、特に施設の設計段階や、ロボットマニピュレータを新たに設置する場合等に特に有効である。なお、本実施例によるステップＳ５０、ステップＳ５１による処理は、第９の実施例の剛体リンク機構モデルを用いた移動体軌道生成方法に対して適用することも可能である。
【００９４】
以上本発明の実施例について図を参照して説明してきたが、上記複数の実施例に説明した特徴を任意に組み合わせたところの構成であってもよい。
【００９５】
また、各実施例においては移動体がロボットマニピュレータの手先であるものとして説明したが、本発明はこれに限定されない。例えば、水中遊泳ロボット、歩行型ロボット、車輪型ロボット等の、障害物が存在する３次元空間における各種の移動体の軌道生成に関して、本発明を適用することが可能である。また、実施例中でロボットマニピュレータの手先以外の部分と障害物との干渉等に着目したものもあるが、これらもロボットマニピュレータに限定されず、移動体が常に本体等と連結されているような装置等に対して適用することが可能である。例えば、上述した各種のロボットがケーブルで電源等と連結されている装置等に適用することができる。
【００９６】
また、各実施例において説明した移動体軌道生成方法は、各処理を行う機能が実装された入力装置、演算装置、出力装置からなるシステムで実行することが可能であり、例えば各処理を行うためのプログラムを組み込んだ一般的なパーソナルコンピュータを用いることができる。
【図面の簡単な説明】
【００９７】
【図１】本発明の第１の実施例による移動体動作軌道生成方法のフローチャート。
【図２】障害物に基づいて設定される斥力ベクトル場の概要を示す概念図。
【図３】動作軌道モデルを構成する連成バネマス構造の概念図。
【図４】（ａ）動作軌道生成処理前の動作軌道モデルの状態を示す概念図。（ｂ）動作軌道生成処理後の動作軌道モデルの状態を示す概念図。
【図５】第２の実施例による動作軌道モデルを構成する連成バネマスダンパ構造の概念図。
【図６】第５の実施例において動作軌道を生成できない状態を示す概念図。
【図７】第６の実施例による移動体動作軌道生成方法の概念図。
【図８】第７の実施例による移動体動作軌道生成方法のフローチャート。
【図９】第７の実施例において障害物の移動前の状態を示す概念図。
【図１０】第７の実施例において障害物が移動した状態を示す概念図。
【図１１】第８の実施例において障害物の移動に伴い動作軌道を再生成した状態を示す概念図。
【図１２】第８の実施例による移動体動作軌道生成方法のフローチャート。
【図１３】第９の実施例による移動体動作軌道生成方法のフローチャート。
【図１４】第１０の実施例による移動体動作軌道生成方法のフローチャート。
【符号の説明】
【００９８】
１０ 障害物
１１、１１ａ、１１ｂ 斥力ベクトル
２０ 引っ張りバネ
２１ ヒンジバネ
２２ バネマス
２３、２３ａ、２３ｂ、２３ｃ 質点
２４ ダンパ
３０ 引力源
３１ 引力ベクトル
【技術分野】
【０００１】
本発明は、障害物の存在する環境で移動する物体の最適な軌道を生成する移動体軌道生成方法および移動体軌道生成装置に係る。
【背景技術】
【０００２】
従来、ロボットの動作設計にはオフラインティーチングによる動作記録方式若しくは制御プログラムによる方式がある。動作記録方式は組立作業や溶接作業などあらかじめ決められた動作を行う産業用ロボットに広く用いられており、実際の作業環境に合わせた動作の設計が可能であるという利点がある。その一方で、ティーチングに多くの時間がかかると共に、動作の設計に熟練を要するといった欠点がある。制御プログラムによる方式はセンサ情報を用いて判断を行い、ある程度環境にロバストな動作設計を行なうことが可能である。しかし、干渉を回避するための軌道の生成を、シミュレーション等を利用して確認しながら試行錯誤的に行う必要があり、設計と検証に多大な労力が必要となる。
【０００３】
これに対して、ロボットの動作設計の自動化の試みとしてはポテンシャル場を用いた経路探索手法（例えば、特許文献１参照。）がある。この手法では作業空間内に目標位置が最小、現在位置が最大となるような仮想的なポテンシャルを設定する。さらに、障害物にも大きなポテンシャルを持たせることで、ポテンシャルが低くなる方向へ軌道を生成することで、障害物を回避しながら目標位置までの経路を生成できる。
【特許文献１】特開２００３−２８０７１０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
上述したポテンシャル場を用いた経路探索手法では、ポテンシャル場にローカルミニマムが生じて、経路探索時にローカルミニマムに陥ると軌道を生成できないという問題があった。ローカルミニマムとは、ポテンシャルが局所的に極小となる領域を指す。経路探索時にローカルミニマムの領域に入ると、周囲のポテンシャルがローカルミニマムの領域より高く、経路探索を続行できずに軌道生成不能となるのである。
【０００５】
複雑形状の障害物がある環境では、ローカルミニマムの存在しないポテンシャル場を設定することは非常に困難である。また、ローカルミニマムを回避するために迂回した経路を生成するとなると、軌道が長大になり、不要な変曲点を含む軌道となるなど、最適な軌道とならず、軌道の最適化を図る必要等があった。
【０００６】
上述した課題に鑑み、本発明の目的は、ロボットマニピュレータの手先等の目標軌道を試行錯誤的に行うことなく生成でき、かつ、上述したローカルミニマムの問題が生じない移動体軌道生成方法および移動体軌道生成装置を提供することにある。
【課題を解決するための手段】
【０００７】
上記目的を達成するため、本発明による移動体軌道生成方法は、移動体が移動する空間を設定する空間設定ステップと、前記空間中に前記移動体が動作を開始する初期位置を設定する初期位置設定ステップと、前記空間中に前記移動体の目標位置を設定する目標位置設定ステップと、前記空間中に障害物の位置を設定する障害物設定ステップと、前記初期位置と前記目標位置とを結び、前記初期位置と前記目標位置の間に質点を有し、前記初期位置と前記目標位置に両端を固定された動作軌道モデルを設定する動作軌道モデル設定ステップと、前記空間に前記障害物を発生源とする斥力ベクトルからなる斥力ベクトル場を設定する斥力ベクトル場設定ステップと、前記斥力ベクトル場の前記斥力ベクトルを前記質点に作用させ、変形した前記動作軌道モデルを前記移動体の動作軌道とする動作軌道生成ステップと、を備えることを特徴とする。
【０００８】
また、本発明による移動体軌道生成装置は、移動体が移動する空間を設定する空間設定手段と、前記空間中に前記移動体が動作を開始する初期位置を設定する初期位置設定手段と、前記空間中に前記移動体の目標位置を設定する目標位置設定手段と、前記空間中に障害物の位置を設定する障害物設定手段と、前記初期位置と前記目標位置とを結び、前記初期位置と前記目標位置の間に質点を有し、前記初期位置と前記目標位置に両端を固定された動作軌道モデルを設定する動作軌道モデル設定手段と、前記空間に前記障害物を発生源とする斥力ベクトルからなる斥力ベクトル場を設定する斥力ベクトル場設定手段と、前記斥力ベクトル場の前記斥力ベクトルを前記質点に作用させ、変形した前記動作軌道モデルを前記移動体の動作軌道とする動作軌道生成手段と、を備えることを特徴とする。
【発明の効果】
【０００９】
本発明の移動体軌道生成方法および移動体軌道生成装置によれば、従来のポテンシャル法による軌道生成に比較して、ローカルミニマムの問題が生ずることなく移動体の最適な目標軌道を生成することができる。
【発明を実施するための最良の形態】
【００１０】
以下本発明の実施例について図面を参照しながら説明する。
【実施例１】
【００１１】
本発明の第１の実施例について、図面を用いて以下説明する。図１は、本実施例による移動体軌道生成方法の主要なステップを記載したフローチャートである。本実施例の移動体軌道生成方法において行われる処理について、図１を用いて説明する。なお、各実施例においては、軌道が生成される移動体はロボットマニピュレータの手先であるものとして以下説明する。
【００１２】
まず、ロボットマニピュレータが作業を行う作業空間を定義する処理を行う（ステップＳ１）。作業空間とは、ロボットマニピュレータの初期位置や作業位置、および初期位置と作業位置の中間を含む実際の空間の情報に基づいて構築される空間である。この作業空間は、作業を行う施設等の設計情報、例えば３次元ＣＡＤのデータを用いて設定する、またはカメラの映像や各種センサによる計測等で収集したデータを用いて設定する、あるいは手動で設定する、といったことが考えられる。
【００１３】
具体的には、例えばロボットマニュピレータの操作装置が関連付けられ、各種３次元形状データを作成、記憶、修正可能な３次元ＣＡＤシステムを構成する演算装置及び入力装置によって行われる。すなわち、上述の入力装置に施設等の設計情報や収集したデータを入力する、あるいは任意に施設情報等を入力して、上述した作業空間に対応する３次元形状データを設定することによって行われる。
【００１４】
次に、定義された作業空間内に存在する障害物の位置と形状を定義する処理を行う（ステップＳ２）。この障害物の定義も、作業空間の定義と同様に、設計情報やデータ収集、手動設定等によって行うことができる。また、障害物が作業空間中を移動する場合は、障害物の移動情報を反映させて移動する障害物を設定することもできる。
【００１５】
次に、作業空間上に斥力ベクトル場を生成する処理を行う（ステップＳ３）。斥力ベクトル場の生成は、作業空間上に３次元の格子を設定し、各格子点に斥力ベクトルを定義することで行う。この斥力ベクトルはステップＳ２で定義された障害物を発生源として設定する。各格子点に設定される斥力ベクトルの方向は障害物表面の法線方向、すなわち障害物表面に垂直な方向とし、斥力ベクトルの大きさは障害物に近いほど大きくなるように設定する。作業空間上に複数の障害物が存在する場合、各格子点に各障害物から発生する斥力ベクトルを合算したものが、最終的にその格子点に設定される斥力ベクトルとなる。
【００１６】
ステップＳ２で定義された障害物と、ステップＳ３で生成された斥力ベクトル場の例を図２に示す。図２は作業空間に設定された障害物と斥力ベクトル場の概要を示す概念図である。なお、斥力ベクトル場については、斥力ベクトルのおおよその方向のみ示しており、作業空間上に設定される３次元格子は省略している。作業空間中に円筒形状の障害物１０が設定されており、この障害物１０から遠ざかる方向の斥力ベクトル１１が設定されている。
【００１７】
斥力ベクトル場を設定する際、障害物１０が複雑な形状で、突起や極端な狭隘部を有する等のロボットマニピュレータの手先が通過しにくい形状や配置を取る場合は、該当箇所付近の斥力ベクトル１１を大きくする等の重み付けを行ってもよい。
【００１８】
次に、移動体の初期位置と目標位置を結ぶように、動作軌道モデルを作業空間上に生成・配置する処理を行う（ステップＳ４）。この動作軌道モデルの構造の一例を、図３を用いて説明する。動作軌道モデルは、引っ張りバネ２０とヒンジバネ２１を組み合わせたバネマス２２を複数結合し、バネマス２２同士の結合部には質点２３が設定された連成バネマス構造を有する。また、動作軌道モデルの両端に設定される質点２３ａ、質点２３ｂは、動作軌道モデルの配置の際、質点２３ａは移動体の初期位置に、質点２３ｂは移動体の目標位置に固定される。なお、図３ではバネマス２２を２つ結合したものとして図示しているが、バネマス２２の数は任意に設定することができる。
【００１９】
なお、図３においては、移動体の初期位置と目標位置とを直線的に結ぶように動作軌道モデルを配置しているが、直線的に結ぶ必要はなく、空間中で初期位置と目標位置とを任意に結ぶように配置されていればよい。
【００２０】
次に、ステップＳ４で生成した動作軌道モデルにステップＳ３で設定した斥力ベクトル１１を作用させ、動作軌道を生成する処理を行う（ステップＳ５）。動作軌道モデルに設定された各質点２３について、各質点２３周辺に設定された複数の斥力ベクトル１１の平均となるベクトルを作用させる。すると、動作軌道モデルを構成する引っ張りバネ２０、ヒンジバネ２１が変形する。この際、ヒンジバネ２１は隣接する質点２３間を直線形状に保つ方向に力を作用させ、ロボットマニピュレータの動作軌道３１が必要以上に湾曲することを防ぐ。
【００２１】
最終的には、各質点２３に作用する斥力ベクトル１１と引っ張りバネ２０、ヒンジバネ２１の張力がつりあう形状に動作軌道モデルが変形する。この変形した動作軌道モデルの描く軌道がロボットマニピュレータの動作軌道となる。なお、質点２３周辺に斥力ベクトル１１を作用させる際、斥力ベクトル１１が設定された各格子点と斥力ベクトル１１の質点２３との距離に基づいて、複数の斥力ベクトル１１に対して重み付けを行ってベクトルを計算することもできる。
【００２２】
ステップＳ５における動作軌道の生成の例を、図４を用いて説明する。図４（ａ）は動作軌道モデルに斥力ベクトル１１を作用させていない状態、図４（ｂ）は動作軌道モデルに斥力ベクトル１１を作用させて動作軌道を生成した状態を示している。なお、図４においては簡略的に２次元図で表し、各質点２３を結ぶバネマス２２も簡略化して図示している。
【００２３】
図４（ａ）で示すようにステップＳ４において動作軌道モデルが障害物１０を貫通するように配置される。なお、上述したように、質点２３ａは移動体の初期位置に、質点２３ｂは移動体の目標位置に固定されている。この動作軌道モデルに斥力ベクトル１１を作用させると、図４（ｂ）で示す形状に変形し、障害物１０を紙面右側から迂回する動作軌道が生成される。質点２３ａ、質点２３ｂは位置が固定されているため移動しない。
【００２４】
次に、ステップＳ５で生成した動作軌道について、動作軌道モデルの各質点２３間を曲線補間し、動作軌道を滑らかにする処理を行う（ステップＳ６）。
【００２５】
以上の一連の処理により、ロボットマニピュレータの手先の動作軌道が生成される。このような動作軌道生成によれば、ポテンシャル法におけるローカルミニマムの問題が生じることなく、移動体の最適な動作軌道を生成することが可能である。また、動作軌道モデルに連成バネマス構造を採用し、各バネのバネ定数を任意に設定可能とすることで、障害物の配置や形状に応じ、障害物に対する移動体の回避軌道の大きさを任意に変更することが可能となる。
【００２６】
また、ステップＳ１について説明したとおり移動する障害物を設定する場合は、リアルタイムに斥力ベクトル場と動作軌道モデルの形状を更新する処理を行うことも可能である。また、障害物の移動範囲が決まっている場合は、障害物の移動範囲全体を一つの障害物として設定することも可能である。
【００２７】
また、本発明の移動体動作軌道生成方法は、設計情報や、実際の作業環境の情報を収集して設定することができるため、設計・計画段階の施設、建設済みの施設の何れについても適用することが可能である。
【００２８】
また、動作軌道モデルを構成する引っ張りバネ２０、ヒンジバネ２１のバネ定数は任意に設定することができる。斥力ベクトル場が同一の場合、バネ定数が大きいほど動作軌道モデルは変形し難く、逆にバネ定数が小さいほど動作軌道モデルは変形し易くなる。したがって、例えば、手先が大きい場合はバネ定数を小さくすることにより、バネ定数が固定の場合の動作軌道よりも障害物１０から離れた動作軌道が生成される。逆に、手先が小さい場合はバネ定数を大きくすることでバネ定数が固定の場合の動作軌道よりも障害物１０に近い動作軌道が生成される。換言すれば、バネ定数を任意に設定することにより、障害物１０に対する移動体の回避軌道の大きさを任意に変更することが可能である。
【００２９】
つまり、上述の方法により手先の大きさに合わせてロボットマニピュレータと障害物１０の干渉回避距離を確保した動作軌道を生成することができる。また、動作軌道モデルを構成する各バネのバネ定数は同一でなくともよい。例えば、障害物１０が少ない箇所におけるバネ定数を大きくし、障害物１０が多く狭隘な箇所におけるバネ定数を小さくするなど、作業空間内１の障害物１０の形状に合わせて部分的にバネ定数を変えることで、バネ定数が大きい箇所では障害物１０の形状に依存することなく、より直線的な軌道を得ることができ、バネ定数が小さい箇所では障害物１０の形状に沿った干渉回避軌道を得ることができる。
【００３０】
また、動作軌道を生成する際に、質点２３の各々についてロボットマニピュレータの可動範囲を拘束条件としてもよい。すなわち、質点２３の各々は斥力ベクトル１１を作用させることで作業空間中を移動するが、この際にロボットマニピュレータの可動範囲外には移動できないよう設定することで、確実にロボットマニピュレータの可動範囲内に動作軌道を生成することが可能である。
【００３１】
また、本実施例においては動作軌道モデルに関して、間隔を空けて質点２３を離散的に配置したものとして説明したが、連続的に質点２３を配置したものとして、つまり動作軌道モデルが全体として質量を有するものとすることも可能である。
【実施例２】
【００３２】
本発明の第２の実施例について、図面を用いて以下説明する。なお、第１の実施例と同じ構成には同一の符号を付し、重複する説明は省略する。
【００３３】
本実施例においては、ステップＳ４で生成される動作軌道モデルに、図５に示す連成バネマスダンパ構造を採用している。この連成バネマスダンパ構造は、連成バネマス構造の質点２３と引っ張りバネ２０の間にダンパ２４を追加した構造である。
【００３４】
この連成バネマスダンパ構造を採用することにより、ステップＳ５で動作軌道モデルに斥力ベクトル１１を作用させて軌道を生成する際に、動作軌道モデル変形時の各質点２３間の振動、つまり引っ張りバネ２０の伸縮方向の振動を抑制することができる。
【００３５】
また、各ダンパ２４の減衰係数を可変とし、動的に位置が変動する障害物１０等により瞬間的に過大な斥力が作用した場合にダンパ２４の減衰係数が増加する設定、すなわちダンパ２４に印加される応力の増加に応じて減衰係数が増加するよう設定することで、動作軌道が部分的に大きく変形することを防止することができる。
【００３６】
また、各ダンパの減衰係数を個別に設定可能とし、たとえば移動体の初期位置と目標位置付近のダンパは減衰係数を大きくして動作軌道の変形を抑制し、他のダンパは減衰係数を小さくして動作軌道を変形しやすくする等、動作軌道モデルのバネ定数を個別に異なる設定とした場合と類似した効果を得ることができる。
【００３７】
なお、当然ながら各質点２３間の全箇所にダンパ２４を設定する必要はなく、任意の箇所にのみダンパを追加した連成バネマスダンパ構造とすることも可能である。また、質点２３と引っ張りバネ２０の間にダンパ２４を追加したものとして説明したが、当然ヒンジバネ２１と質点２３の間、あるいは引っ張りバネ２０とヒンジバネ２１の間にダンパ２４を追加することも可能である。
【実施例３】
【００３８】
本発明の第３の実施例について以下説明する。
【００３９】
本実施例の移動体動作軌道生成方法では、動作軌道モデルを生成するステップＳ４において、各質点２３に粘性摩擦係数を設定する。ここでいう各質点２３の粘性摩擦とは、作業空間上を質点２３が移動する際に、質点２３の移動に対して抵抗力として働くものである。すなわち、この粘性摩擦係数が高いほど質点２３は移動しにくくなる。この粘性摩擦係数を設定することにより、動作軌道モデルに斥力ベクトル１１を作用させて動作軌道を生成する際の動作軌道モデルの振動を抑制することが可能である。質点２３の粘性摩擦は全方向の移動に対して作用するため、連成バネマスダンパ構造のダンパと異なり、動作軌道モデル全体のあらゆる方向の振動を抑制することが可能である。
【００４０】
この粘性摩擦係数は、第２の実施例で説明したダンパの減衰係数と同様に、質点２３に働く斥力の増加にともなって増加する設定とすることができ、また、各質点２３について個別に設定することが可能である。
【実施例４】
【００４１】
本発明の第４の実施例について以下説明する。
【００４２】
本実施例においては、動作軌道モデルを生成するステップＳ４において、生成される動作軌道モデルを構成するバネの自然長を個別に設定し、各質点２３間の距離を個別に設定可能とするものである。また、ステップＳ５で生成された動作軌道について、移動体の動作速度を各質点２３間の距離と関連付けることができ、例えば生成された動作軌道について、動作軌道モデルの各質点２３間の移動に要する時間を一定とした場合、質点２３間の距離が長い場合は移動体の動作速度が速く、質点２３間の距離が短い場合は移動体の動作速度を遅くするよう設定することができる。
【００４３】
さらに、上述した実施例と組み合わせ、例えばバネ定数の個別設定によって障害物１０に対して大きく回避軌道をとる設定とした箇所では移動体の動作速度を速く、障害物１０に対して小さく回避軌道を取る設定とした箇所では移動体の動作速度を遅くする、といった設定が可能である。
【００４４】
本実施例によれば、動作軌道の生成時に、移動体の動作速度も自動で最適な設定とすることが可能である。
【実施例５】
【００４５】
本発明の第５の実施例について、図面を用いて以下説明する。
【００４６】
本実施例においては、軌道生成ステップＳ５にて動作軌道を生成する際に、動作軌道モデルの任意の質点２３を微小移動させる質点移動処理を行うことができる。
【００４７】
作業空間上に設定された障害物１０、移動体の初期位置、目標位置によっては、動作軌道モデルの各質点２３に作用する斥力ベクトルが釣り合ってしまい、ステップＳ５にて動作軌道モデルの各質点２３に斥力を作用させても動作軌道モデルが変形せず、障害物１０を回避する軌道が生成されない状況が考えられる。
【００４８】
このような状況の一例を図６に示す。図６では、動作軌道モデルの各質点２３に対して、紙面右方向に作用する斥力１１ａと紙面左方向に作用する斥力１１ｂが釣り合った状態を示している。図６では２次元的に示しているが、３次元の作業空間上で各質点２３に作用する斥力ベクトルが釣り合ってしまうと、図６のように動作軌道モデルが障害物１０を貫通する状態で維持されることになる。
【００４９】
そこで、本実施例においては、上述したように軌道生成ステップＳ５において、動作軌道モデルの各質点２３に作用する斥力ベクトルが釣り合った場合に、任意の質点２３を微小移動させる質点移動処理を行う。この質点移動処理により、質点２３に作用する斥力の均衡を崩し、ステップＳ５において正常に動作軌道を生成することが可能である。
【実施例６】
【００５０】
本発明の第６の実施例について、図面を用いて以下説明する。図７は実施例５の別の形態の概念図であり、作業空間中に引力源３０が設定されている。なお、破線は変形前の動作軌道モデルの配置を示している。
【００５１】
図７に示した形態では、ステップＳ５において作業空間の任意の位置に引力源３０を設定する処理を行う。引力源３０は、図７に示すように、各質点２３に対して引力源３０の方向へ働く引力ベクトル３１を与える。各質点２３に引力ベクトル３１を作用させることにより、生成される動作軌道について、引力源３０側の軌道を描くように制御することが可能である。例えば、ロボットマニピュレータ本体の設置位置に引力源を設定することにより、動作軌道をロボットマニピュレータに近い側に生成させることができる。あるいは、実際の作業空間に設置されたカメラに対応する位置に引力源３０を設定することにより、ロボットマニピュレータを監視し易い動作軌道とすることができる。
【００５２】
また、引力源３０を設定することにより、第５の実施例で説明したような斥力ベクトルの均衡によって動作軌道が正常に生成されない状況を解消することが可能である。
【実施例７】
【００５３】
本発明の第７の実施例について、図面を用いて以下説明する。なお、第１の実施例と同じ構成には同一の符号を付し、重複する説明は省略する。
【００５４】
図８は、本実施例による移動体軌道生成方法の主要なステップを記載したフローチャートである。本実施例による移動体軌道生成方法は障害物が移動する作業環境を想定したもので、動作軌道モデルを生成・配置した後の処理が第１の実施例と異なっている。
【００５５】
作業空間の定義（ステップＳ１）から動作軌道モデルの生成・配置（ステップＳ４）までは第１の実施例と同様の処理を行う。
【００５６】
次に、軌道生成（ステップＳ１５）、曲線補間（ステップＳ１６）を行う。これらの処理も、この段階では第１の実施例の軌道生成（ステップＳ５）、曲線補間（ステップＳ６）と同様の処理である。
【００５７】
次に、作業空間に配置された障害物１０を移動する処理を行う（ステップＳ１７）。
【００５８】
次に、障害物１０の移動に伴う斥力ベクトル場の更新を行う。また、斥力ベクトル場の更新に合わせて動作軌道モデルに作用する斥力ベクトルを更新し、動作軌道モデルを更新する処理を行う（ステップＳ１８）。
【００５９】
次に、ステップＳ１８で更新された動作軌道モデルについて、動作軌道モデルを構成する各バネの張力が所定の閾値を超えているかいないかを判定する処理を行う（ステップＳ１９）。
【００６０】
ステップＳ１９の判定で、バネ張力が閾値より低かった場合は、移動体の移動を行う（ステップＳ２０）。そして、移動体が目標位置に到達したかどうか判定を行い（ステップＳ２１）、到達していた場合は処理を終了し、到達していない場合はステップＳ１７に戻り、障害物１０の移動以降のステップを繰り返し行う。
【００６１】
ステップＳ１９の判定で、バネ張力が閾値よりも高かった場合は、動作軌道モデルを再配置する処理を行う（ステップＳ２２）。この動作軌道モデルの再配置は、更新後の斥力ベクトル場の他、移動体の現在位置を考慮する点でステップＳ１４と異なる。ステップＳ１４では移動体の初期位置と目標位置のみを動作軌道モデルの拘束点として配置しているが、ステップＳ２２では移動体の初期位置、目標位置に加えて移動体の現在位置を拘束点として動作軌道モデルを再配置する。この動作軌道モデルの再配置後は、再度軌道生成（ステップＳ１５）、曲線補間（ステップＳ１６）を行い、障害物１０の移動以降のステップを繰り返す。
【００６２】
このような処理とすることにより、障害物１０の移動にともなって動作軌道を逐次更新することが可能である。また、動作軌道モデルを構成するバネの張力を判定することにより、障害物１０の移動によって更新された動作軌道モデルが極端な軌道を描く場合等に、動作軌道モデルの再設定を行うことによって、移動体の現在位置を考慮した最適な動作軌道への変更を自動で行うことが可能である。
【００６３】
本実施例による動作軌道の変更処理の例を、図面を用いて説明する。図９、図１０、図１１はそれぞれ、本実施例によるステップＳ１７以降の処理を説明するための概念図であり、図９は障害物１０が移動する前の状態、図１０は障害物１０が移動した後の状態、図１１はステップＳ２２を経て動作軌道を変更した例を示している。
【００６４】
図９はステップＳ１５で最初に動作軌道を生成した状態を示している。障害物１０が複雑な形状をしており、動作軌道モデルがこの障害物１０の凹部を通過するように迂回する軌道をとっている。矢印４０はこの障害物１０の移動方向を示し、破線は移動体の初期位置と目標位置とを直線的に結んだもので、変形前の動作軌道モデルの位置に相当する。
【００６５】
図１０はステップＳ１７によって障害物１０が移動し、ステップＳ１８で斥力ベクトル場と動作軌道とが更新された状態を示している。図９の状態と比較すると、障害物１０が矢印４０方向へ移動したことにより、動作軌道モデルが障害物１０をより大きく迂回するような軌道をとっていることが分かる。
【００６６】
図１１はステップＳ１９で動作軌道モデルのバネ張力が閾値より高いと判定され、ステップＳ２２の動作軌道モデルの再配置を経てステップＳ１５で動作軌道が変更された状態を示している。なお、図１１ではステップＳ２２の時点で移動体が質点２３ｃの位置まで移動していたものとして図示している。よって、移動体の初期位置、目標位置のほか、質点２３ｃの位置も動作軌道モデルの拘束点となっており、動作軌道が変更された結果、図９や図１０とは逆側から障害物１０を迂回するような軌道をとっている。なお、図１１においては、破線はステップＳ１９で判定された際の動作軌道モデルの形状を示している。
【００６７】
なお、本実施例によってはステップＳ１９において動作軌道モデルを構成するバネの張力に基づいて動作軌道モデルを再配置する処理の実行を判定するものとしたが、例えば動作軌道モデルがロボットマニピュレータの可動範囲から逸脱した場合に動作軌道モデルを再配置するような判定とすることも可能である。
【実施例８】
【００６８】
本発明の第８の実施例について、図面を用いて以下説明する。なお、第１の実施例と同じ構成には同一の符号を付し、重複する説明は省略する。
【００６９】
図１２は、本実施例による移動体軌道生成方法の主要なステップを記載したフローチャートである。本実施例による移動体軌道生成方法は、移動体であるロボットマニピュレータの手先の移動中におけるロボットマニピュレータと障害物との干渉を想定したもので、動作軌道モデルを生成した後の処理が第１の実施例と異なっている。
【００７０】
作業空間の定義（ステップＳ１）から動作軌道モデルの生成・配置（ステップＳ４）までは第１の実施例と同様の処理を行う。
【００７１】
次に、軌道生成（ステップＳ２５）、曲線補間（ステップＳ２６）を行う。これらの処理も、この段階では第１の実施例の軌道生成（ステップＳ５）、曲線補間（ステップＳ６）と同様の処理である。ただし、ロボットマニピュレータの設置位置が移動体の初期位置と異なる場合は、別途にロボットマニピュレータの設置位置を設定する処理を行う。
【００７２】
次に、移動体であるロボットマニピュレータの手先軌道から逆運動学を解き、ロボットマニピュレータの関節角度を算出し、ロボットマニピュレータの姿勢を計算する処理を行う（ステップＳ２７）。この計算には、ロボットマニピュレータの設置位置、手先の位置、ロボットマニピュレータの関節数や大きさ、形状等の情報を用いて行う。この際、手先が動作軌道の移動体初期位置から目標位置にかけて移動する間の任意の位置または全位置についてロボットマニピュレータ姿勢計算を行う。
【００７３】
次に、ステップＳ２７で計算したロボットマニピュレータの姿勢が障害物１０と干渉するかどうか判定する処理を行う（ステップＳ２８）。干渉すると判定された場合、ロボットマニピュレータが干渉する際の手先位置に近い質点２３に作用する斥力ベクトル１１をより大きな値に再設定する処理を行う（ステップＳ２９）。斥力ベクトル１１の再設定後、ステップＳ２５で動作軌道を再生成し、以降の処理を繰り返す。ステップＳ２８でロボットマニピュレータと障害物１０が干渉しないと判定された場合は、動作軌道の生成を終了する。
【００７４】
以下、本実施例による移動体軌道生成方法の効果について説明する。
【００７５】
各実施例で説明した移動体軌道生成方法によれば、障害物１０と干渉することがなく最適な移動体の動作軌道を生成することができる。しかし、移動体がロボットマニピュレータの手先である場合では、手先が障害物１０を回避する軌道であったとしても、ロボットマニピュレータの腕が障害物１０と干渉することが考えられる。そこで、手先が移動する間のロボットマニピュレータ自身の姿勢を逆運動学によって算出し、障害物１０とロボットマニピュレータとの干渉を回避する動作軌道を生成することが可能である。
【実施例９】
【００７６】
本発明の第９の実施例について、図面を用いて以下説明する。なお、第１の実施例と同じ構成には同一の符号を付し、重複する説明は省略する。
【００７７】
図１３は、本実施例による移動体軌道生成方法の主要なステップを記載したフローチャートである。本実施例による移動体軌道生成方法は、移動体であるロボットマニピュレータの手先の移動中におけるロボットマニピュレータと障害物との干渉を想定したもので、動作軌道モデルを生成した後の処理が第１の実施例と異なっている。
【００７８】
作業空間の定義（ステップＳ１）から動作軌道モデルの生成・配置（ステップＳ４）までは第１の実施例と同様の処理を行う。
【００７９】
次に、軌道生成（ステップＳ３５）、曲線補間（ステップＳ３６）を行う。これらの処理も、この段階では第１の実施例の軌道生成（ステップＳ５）、曲線補間（ステップＳ６）と同様の処理である。
【００８０】
次に、剛体リンク機構モデルを生成・配置する処理を行う（ステップＳ３７）。この剛体リンク機構モデルとはロボットマニピュレータをモデル化したもので、実際に使用するロボットマニピュレータと同数の関節を有し、関節と関節とを剛体のリンクで連絡した仮想的なモデルである。この剛体リンク機構モデルについては、動作軌道モデルの質点２３と同様に、関節とリンクの双方に斥力ベクトル場の斥力ベクトル１１が作用する。
【００８１】
次に、ロボットマニピュレータの姿勢を、剛体リンク機構と剛体リンク機構に作用する斥力ベクトル１１とを用いて計算する処理を行う（ステップＳ３８）。ロボットマニピュレータの姿勢計算は、第８の実施例のステップＳ２７と同様に、手先が動作軌道の移動体初期位置から目標位置にかけて移動する間の任意の位置または全位置についてロボットマニピュレータ姿勢計算を行う。
【００８２】
次に、ステップＳ３８で計算した剛体リンク機構の形状に基づき、手先が移動後位置に移動した際にロボットマニピュレータと障害物１０とが干渉するかを判定する処理を行う（ステップＳ３９）。干渉する場合は第８の実施例のステップＳ２９と同様に、斥力ベクトル１１を再設定し（ステップＳ４０）、ステップＳ３５で動作軌道を再生成して以降の処理を繰り返す。
【００８３】
ステップＳ３９でロボットマニピュレータと障害物１０が干渉しないと判定された場合は、動作軌道の生成を終了する。
【００８４】
本実施例によれば、第８の実施例で説明した移動体軌道生成方法と同様に、障害物１０とロボットマニピュレータとの干渉を回避する動作軌道を生成することが可能である。また、本実施例によれば、剛体リンク機構を採用したことにより、手先位置と剛体リンク機構に作用する斥力ベクトルからロボットマニピュレータの姿勢を計算することが可能である。したがって、ロボットマニピュレータの自由度が冗長で、逆運動学の解が一意に導き出せないような場合にも適用可能である。
【実施例１０】
【００８５】
本発明の第１０の実施例について、図面を用いて以下説明する。なお、第８の実施例と同じ構成には同一の符号を付し、重複する説明は省略する。
【００８６】
図１４は、本実施例による移動体軌道生成方法の主要なステップを記載したフローチャートである。本実施例による移動体軌道生成方法は、第８の実施例と同様に、移動体であるロボットマニピュレータの手先の移動中におけるロボットマニピュレータと障害物との干渉を想定したもので、障害物１０とロボットマニピュレータの干渉を判定した後の処理が第８の実施例と異なっている。
【００８７】
作業空間の定義（ステップＳ１）から、障害物１０とロボットマニピュレータの干渉の判定（ステップＳ２８）までは第８の実施例と同様の処理を行う。
【００８８】
次に、ステップＳ２８で干渉すると判定された場合に、斥力ベクトル１１を再設定するステップＳ２９の前に、斥力ベクトル１１の再設定回数の累積が閾値以上かを判定する処理を行う（ステップＳ５０）。
【００８９】
ステップＳ５０で、斥力ベクトル１１の再設定回数の累積が閾値未満の場合、ステップＳ２９に進み、斥力ベクトル１１の再設定を行う。斥力ベクトル１１の再設定回数の累積が閾値以上の場合、ロボットマニピュレータの設置位置を任意の位置に変更して再設定する処理を行う（ステップＳ５１）。この際、斥力ベクトル１１の再設定回数はリセットする。ロボットマニピュレータの設置位置を変更した後、ステップＳ４以降の処理を繰り返す。
【００９０】
本実施例の効果について、以下説明する。
【００９１】
第８の実施例について、斥力ベクトル１１を再設定してもロボットマニピュレータと障害物１０との干渉が解消されない場合、斥力ベクトル１１の再設定を繰り返し行うことになる。この場合、斥力ベクトル１１の再設定を繰り返しても干渉が回避されず処理が無限ループに陥る場合や、干渉が解消されたとしても動作軌道が極端に大きな回避軌道をとってしまう場合等が考えられる。
【００９２】
本実施例では、斥力ベクトル１１の再設定回数に累積閾値を設定して、閾値に達するまでの再設定で干渉が解消されない場合に、ロボットマニピュレータの初期位置変更を行う。これによって、上述したような無限ループや、極端な動作軌道が生成されることを防止することが可能である。
【００９３】
本実施例はロボットマニピュレータの設置位置を変更するものであるため、特に施設の設計段階や、ロボットマニピュレータを新たに設置する場合等に特に有効である。なお、本実施例によるステップＳ５０、ステップＳ５１による処理は、第９の実施例の剛体リンク機構モデルを用いた移動体軌道生成方法に対して適用することも可能である。
【００９４】
以上本発明の実施例について図を参照して説明してきたが、上記複数の実施例に説明した特徴を任意に組み合わせたところの構成であってもよい。
【００９５】
また、各実施例においては移動体がロボットマニピュレータの手先であるものとして説明したが、本発明はこれに限定されない。例えば、水中遊泳ロボット、歩行型ロボット、車輪型ロボット等の、障害物が存在する３次元空間における各種の移動体の軌道生成に関して、本発明を適用することが可能である。また、実施例中でロボットマニピュレータの手先以外の部分と障害物との干渉等に着目したものもあるが、これらもロボットマニピュレータに限定されず、移動体が常に本体等と連結されているような装置等に対して適用することが可能である。例えば、上述した各種のロボットがケーブルで電源等と連結されている装置等に適用することができる。
【００９６】
また、各実施例において説明した移動体軌道生成方法は、各処理を行う機能が実装された入力装置、演算装置、出力装置からなるシステムで実行することが可能であり、例えば各処理を行うためのプログラムを組み込んだ一般的なパーソナルコンピュータを用いることができる。
【図面の簡単な説明】
【００９７】
【図１】本発明の第１の実施例による移動体動作軌道生成方法のフローチャート。
【図２】障害物に基づいて設定される斥力ベクトル場の概要を示す概念図。
【図３】動作軌道モデルを構成する連成バネマス構造の概念図。
【図４】（ａ）動作軌道生成処理前の動作軌道モデルの状態を示す概念図。（ｂ）動作軌道生成処理後の動作軌道モデルの状態を示す概念図。
【図５】第２の実施例による動作軌道モデルを構成する連成バネマスダンパ構造の概念図。
【図６】第５の実施例において動作軌道を生成できない状態を示す概念図。
【図７】第６の実施例による移動体動作軌道生成方法の概念図。
【図８】第７の実施例による移動体動作軌道生成方法のフローチャート。
【図９】第７の実施例において障害物の移動前の状態を示す概念図。
【図１０】第７の実施例において障害物が移動した状態を示す概念図。
【図１１】第８の実施例において障害物の移動に伴い動作軌道を再生成した状態を示す概念図。
【図１２】第８の実施例による移動体動作軌道生成方法のフローチャート。
【図１３】第９の実施例による移動体動作軌道生成方法のフローチャート。
【図１４】第１０の実施例による移動体動作軌道生成方法のフローチャート。
【符号の説明】
【００９８】
１０ 障害物
１１、１１ａ、１１ｂ 斥力ベクトル
２０ 引っ張りバネ
２１ ヒンジバネ
２２ バネマス
２３、２３ａ、２３ｂ、２３ｃ 質点
２４ ダンパ
３０ 引力源
３１ 引力ベクトル
【特許請求の範囲】
【請求項１】
移動体が移動する空間を設定する空間設定ステップと、
前記空間中に前記移動体が動作を開始する初期位置を設定する初期位置設定ステップと、
前記空間中に前記移動体の目標位置を設定する目標位置設定ステップと、
前記空間中に障害物の位置を設定する障害物設定ステップと、
前記初期位置と前記目標位置とを結び、前記初期位置と前記目標位置の間に質点を有し、前記初期位置と前記目標位置に両端を固定された動作軌道モデルを設定する動作軌道モデル設定ステップと、
前記空間に前記障害物を発生源とする斥力ベクトルからなる斥力ベクトル場を設定する斥力ベクトル場設定ステップと、
前記斥力ベクトル場の前記斥力ベクトルを前記質点に作用させ、変形した前記動作軌道モデルを前記移動体の動作軌道とする動作軌道生成ステップと、
を備えることを特徴とする移動体軌道生成方法。
【請求項２】
前記斥力ベクトル場は、前記障害物との距離が近いほど前記斥力ベクトルが大きくなるよう設定されたことを特徴とする請求項１記載の移動体軌道生成方法。
【請求項３】
前記斥力ベクトル場設定ステップで、前記障害物の形状に基づいて前記斥力ベクトル場に設定される前記斥力ベクトルに重み付けを行うことを特徴とする請求項１乃至請求項２の何れか１項記載の移動体軌道生成方法。
【請求項４】
前記動作軌道モデルは、長さ方向に伸縮する引っ張りバネと、この引っ張りバネに連結され前記引っ張りバネの方向を変化させるヒンジバネと、を備えるバネマス構造と前記質点とを組み合わせた連成バネマス構造であることを特徴とする請求項１乃至請求項３の何れか１項記載の移動体軌道生成方法。
【請求項５】
前記連成バネマス構造は、連結された複数の前記バネマス構造と、前記バネマス構造の各々の連結部に設定された質点と、から構成されることを特徴とする請求項４記載の移動体軌道生成方法。
【請求項６】
前記引っ張りバネ、前記ヒンジバネのうち少なくとも何れかのバネ定数を設定するバネ定数設定ステップを備えることを特徴とする請求項４または請求項５に記載の移動体軌道生成方法。
【請求項７】
前記動作軌道モデルは、前記バネマス構造と前記質点の間に設定されたダンパを備えることを特徴とする請求項４乃至請求項６の何れか１項記載の移動体軌道生成方法。
【請求項８】
前記ダンパに印加される応力の増加に応じて前記ダンパの減衰係数を増加させることを特徴とする請求項７記載の移動体軌道生成方法。
【請求項９】
前記質点に、前記空間上での前記質点の移動に対する抵抗力として作用する粘性摩擦の粘性摩擦係数を設定する粘性摩擦係数設定ステップと、を備えることを特徴とする請求項１乃至請求項８の何れか１項記載の移動体軌道生成方法。
【請求項１０】
前記質点に印加される応力の増加に応じて前記粘性摩擦係数を増加させることを特徴とする請求項９記載の移動体軌道生成方法。
【請求項１１】
前記動作軌道生成ステップで変形した前記動作軌道モデルの前記質点の各々の間隔に応じて前記移動体の動作速度を設定することを特徴とすることを特徴とする請求項１乃至請求項１０の何れか１項記載の移動体軌道生成方法。
【請求項１２】
前記障害物設定ステップで設定される前記障害物が、動的に位置が変動する移動障害物であり、
前記動作軌道生成ステップの後に前記移動障害物が移動する処理を行い、前記移動障害物の移動後に前記斥力ベクトル場および前記動作軌道を更新する更新ステップを備えることを特徴とする請求項１乃至請求項１１の何れか１項記載の移動体軌道生成方法。
【請求項１３】
前記動作軌道モデルの更新後に、前記動作軌道モデルの張力と所定の閾値とを比較する張力判定ステップと、
前記張力判定ステップで前記張力が前記閾値より大きい場合に、前記移動体の前記初期位置、前記目標位置、現在位置を拘束点として動作軌道を再設定する動作軌道変更ステップと、を備えることを特徴とする請求項１２記載の移動体軌道生成方法。
【請求項１４】
前記動作軌道生成ステップで前記質点への前記斥力ベクトルの作用によって前記動作軌道モデルが前記障害物外に設定されない場合に前記質点を一時的に移動させる質点移動処理を行うことを特徴とする請求項１乃至請求項１３の何れか１項記載の移動体軌道生成方法。
【請求項１５】
前記移動体がロボットマニピュレータの手先であり、前記動作軌道モデルの各質点について前記ロボットマニピュレータの可動範囲を拘束条件とする拘束条件付与ステップを備えることを特徴とする請求項１乃至請求項１４の何れか１項記載の移動体軌道生成方法。
【請求項１６】
前記移動体がロボットマニピュレータの手先であり、前記動作軌道生成ステップで生成された前記動作軌道から逆運動学を解いて前記ロボットマニピュレータの姿勢を算出するマニピュレータ姿勢算出ステップを備えることを特徴とする請求項１乃至請求項１５の何れか１項記載の移動体軌道生成方法。
【請求項１７】
前記移動体がロボットマニピュレータの手先であり、前記ロボットマニピュレータと同数の関節と、前記関節同士を剛体のリンクで連絡した剛体リンク機構により前記ロボットマニピュレータをモデル化した剛体リンク機構モデルを生成する剛体リンク機構モデル生成ステップと、
前記剛体リンク機構モデルに前記斥力ベクトルを作用させて前記ロボットマニピュレータの姿勢を算出するマニピュレータ姿勢算出ステップとを備えることを特徴とする請求項１乃至請求項１５の何れか１項記載の移動体軌道生成方法。
【請求項１８】
前記マニピュレータ姿勢算出ステップで算出した前記ロボットマニピュレータの姿勢から前記ロボットマニピュレータと前記障害物が干渉するか判定する干渉判定ステップと、
この干渉判定ステップで干渉すると判定された場合に、前記ロボットマニピュレータと前記障害物とが干渉する姿勢の手先位置近傍の前記質点に作用する前記斥力ベクトルを増加させる斥力ベクトル再設定ステップと、
この斥力ベクトル再設定ステップの後に動作軌道を再生成する動作軌道再生成ステップと、を備えることを特徴とする請求項１６または請求項１７に記載の移動体軌道生成方法。
【請求項１９】
前記斥力ベクトル再設定ステップによる斥力ベクトルの再設定回数の累積と所定の閾値とを比較する再設定回数判定ステップと、
この再設定回数判定ステップで前記再設定回数の累積が前記閾値より多い場合に前記移動体の初期位置を変更する移動体初期位置再設定ステップと、を備えることを特徴とする請求項１８記載の移動体軌道生成方法。
【請求項２０】
前記空間中に前記質点に対して引力ベクトルを作用させる引力源を設定する引力源生成ステップを備えることを特徴とする請求項１乃至請求項１９のうち何れか１項記載の移動体軌道生成方法。
【請求項２１】
前記動作軌道生成ステップで生成された動作軌道について、前記質点間毎に前記移動体の移動速度を決定する移動体移動速度設定ステップを備え、前記質点間の間隔が長いほど前記移動速度が速く設定されたことを特徴とする請求項１乃至請求項２０のうち何れか１項記載の移動体動作軌道生成方法。
【請求項２２】
移動体が移動する空間を設定する空間設定手段と、
前記空間中に前記移動体が動作を開始する初期位置を設定する初期位置設定手段と、
前記空間中に前記移動体の目標位置を設定する目標位置設定手段と、
前記空間中に障害物の位置を設定する障害物設定手段と、
前記初期位置と前記目標位置とを結び、前記初期位置と前記目標位置の間に質点を有し、前記初期位置と前記目標位置に両端を固定された動作軌道モデルを設定する動作軌道モデル設定手段と、
前記空間に前記障害物を発生源とする斥力ベクトルからなる斥力ベクトル場を設定する斥力ベクトル場設定手段と、
前記斥力ベクトル場の前記斥力ベクトルを前記質点に作用させ、変形した前記動作軌道モデルを前記移動体の動作軌道とする動作軌道生成手段と、
を備えることを特徴とする移動体軌道生成方法。
【請求項１】
移動体が移動する空間を設定する空間設定ステップと、
前記空間中に前記移動体が動作を開始する初期位置を設定する初期位置設定ステップと、
前記空間中に前記移動体の目標位置を設定する目標位置設定ステップと、
前記空間中に障害物の位置を設定する障害物設定ステップと、
前記初期位置と前記目標位置とを結び、前記初期位置と前記目標位置の間に質点を有し、前記初期位置と前記目標位置に両端を固定された動作軌道モデルを設定する動作軌道モデル設定ステップと、
前記空間に前記障害物を発生源とする斥力ベクトルからなる斥力ベクトル場を設定する斥力ベクトル場設定ステップと、
前記斥力ベクトル場の前記斥力ベクトルを前記質点に作用させ、変形した前記動作軌道モデルを前記移動体の動作軌道とする動作軌道生成ステップと、
を備えることを特徴とする移動体軌道生成方法。
【請求項２】
前記斥力ベクトル場は、前記障害物との距離が近いほど前記斥力ベクトルが大きくなるよう設定されたことを特徴とする請求項１記載の移動体軌道生成方法。
【請求項３】
前記斥力ベクトル場設定ステップで、前記障害物の形状に基づいて前記斥力ベクトル場に設定される前記斥力ベクトルに重み付けを行うことを特徴とする請求項１乃至請求項２の何れか１項記載の移動体軌道生成方法。
【請求項４】
前記動作軌道モデルは、長さ方向に伸縮する引っ張りバネと、この引っ張りバネに連結され前記引っ張りバネの方向を変化させるヒンジバネと、を備えるバネマス構造と前記質点とを組み合わせた連成バネマス構造であることを特徴とする請求項１乃至請求項３の何れか１項記載の移動体軌道生成方法。
【請求項５】
前記連成バネマス構造は、連結された複数の前記バネマス構造と、前記バネマス構造の各々の連結部に設定された質点と、から構成されることを特徴とする請求項４記載の移動体軌道生成方法。
【請求項６】
前記引っ張りバネ、前記ヒンジバネのうち少なくとも何れかのバネ定数を設定するバネ定数設定ステップを備えることを特徴とする請求項４または請求項５に記載の移動体軌道生成方法。
【請求項７】
前記動作軌道モデルは、前記バネマス構造と前記質点の間に設定されたダンパを備えることを特徴とする請求項４乃至請求項６の何れか１項記載の移動体軌道生成方法。
【請求項８】
前記ダンパに印加される応力の増加に応じて前記ダンパの減衰係数を増加させることを特徴とする請求項７記載の移動体軌道生成方法。
【請求項９】
前記質点に、前記空間上での前記質点の移動に対する抵抗力として作用する粘性摩擦の粘性摩擦係数を設定する粘性摩擦係数設定ステップと、を備えることを特徴とする請求項１乃至請求項８の何れか１項記載の移動体軌道生成方法。
【請求項１０】
前記質点に印加される応力の増加に応じて前記粘性摩擦係数を増加させることを特徴とする請求項９記載の移動体軌道生成方法。
【請求項１１】
前記動作軌道生成ステップで変形した前記動作軌道モデルの前記質点の各々の間隔に応じて前記移動体の動作速度を設定することを特徴とすることを特徴とする請求項１乃至請求項１０の何れか１項記載の移動体軌道生成方法。
【請求項１２】
前記障害物設定ステップで設定される前記障害物が、動的に位置が変動する移動障害物であり、
前記動作軌道生成ステップの後に前記移動障害物が移動する処理を行い、前記移動障害物の移動後に前記斥力ベクトル場および前記動作軌道を更新する更新ステップを備えることを特徴とする請求項１乃至請求項１１の何れか１項記載の移動体軌道生成方法。
【請求項１３】
前記動作軌道モデルの更新後に、前記動作軌道モデルの張力と所定の閾値とを比較する張力判定ステップと、
前記張力判定ステップで前記張力が前記閾値より大きい場合に、前記移動体の前記初期位置、前記目標位置、現在位置を拘束点として動作軌道を再設定する動作軌道変更ステップと、を備えることを特徴とする請求項１２記載の移動体軌道生成方法。
【請求項１４】
前記動作軌道生成ステップで前記質点への前記斥力ベクトルの作用によって前記動作軌道モデルが前記障害物外に設定されない場合に前記質点を一時的に移動させる質点移動処理を行うことを特徴とする請求項１乃至請求項１３の何れか１項記載の移動体軌道生成方法。
【請求項１５】
前記移動体がロボットマニピュレータの手先であり、前記動作軌道モデルの各質点について前記ロボットマニピュレータの可動範囲を拘束条件とする拘束条件付与ステップを備えることを特徴とする請求項１乃至請求項１４の何れか１項記載の移動体軌道生成方法。
【請求項１６】
前記移動体がロボットマニピュレータの手先であり、前記動作軌道生成ステップで生成された前記動作軌道から逆運動学を解いて前記ロボットマニピュレータの姿勢を算出するマニピュレータ姿勢算出ステップを備えることを特徴とする請求項１乃至請求項１５の何れか１項記載の移動体軌道生成方法。
【請求項１７】
前記移動体がロボットマニピュレータの手先であり、前記ロボットマニピュレータと同数の関節と、前記関節同士を剛体のリンクで連絡した剛体リンク機構により前記ロボットマニピュレータをモデル化した剛体リンク機構モデルを生成する剛体リンク機構モデル生成ステップと、
前記剛体リンク機構モデルに前記斥力ベクトルを作用させて前記ロボットマニピュレータの姿勢を算出するマニピュレータ姿勢算出ステップとを備えることを特徴とする請求項１乃至請求項１５の何れか１項記載の移動体軌道生成方法。
【請求項１８】
前記マニピュレータ姿勢算出ステップで算出した前記ロボットマニピュレータの姿勢から前記ロボットマニピュレータと前記障害物が干渉するか判定する干渉判定ステップと、
この干渉判定ステップで干渉すると判定された場合に、前記ロボットマニピュレータと前記障害物とが干渉する姿勢の手先位置近傍の前記質点に作用する前記斥力ベクトルを増加させる斥力ベクトル再設定ステップと、
この斥力ベクトル再設定ステップの後に動作軌道を再生成する動作軌道再生成ステップと、を備えることを特徴とする請求項１６または請求項１７に記載の移動体軌道生成方法。
【請求項１９】
前記斥力ベクトル再設定ステップによる斥力ベクトルの再設定回数の累積と所定の閾値とを比較する再設定回数判定ステップと、
この再設定回数判定ステップで前記再設定回数の累積が前記閾値より多い場合に前記移動体の初期位置を変更する移動体初期位置再設定ステップと、を備えることを特徴とする請求項１８記載の移動体軌道生成方法。
【請求項２０】
前記空間中に前記質点に対して引力ベクトルを作用させる引力源を設定する引力源生成ステップを備えることを特徴とする請求項１乃至請求項１９のうち何れか１項記載の移動体軌道生成方法。
【請求項２１】
前記動作軌道生成ステップで生成された動作軌道について、前記質点間毎に前記移動体の移動速度を決定する移動体移動速度設定ステップを備え、前記質点間の間隔が長いほど前記移動速度が速く設定されたことを特徴とする請求項１乃至請求項２０のうち何れか１項記載の移動体動作軌道生成方法。
【請求項２２】
移動体が移動する空間を設定する空間設定手段と、
前記空間中に前記移動体が動作を開始する初期位置を設定する初期位置設定手段と、
前記空間中に前記移動体の目標位置を設定する目標位置設定手段と、
前記空間中に障害物の位置を設定する障害物設定手段と、
前記初期位置と前記目標位置とを結び、前記初期位置と前記目標位置の間に質点を有し、前記初期位置と前記目標位置に両端を固定された動作軌道モデルを設定する動作軌道モデル設定手段と、
前記空間に前記障害物を発生源とする斥力ベクトルからなる斥力ベクトル場を設定する斥力ベクトル場設定手段と、
前記斥力ベクトル場の前記斥力ベクトルを前記質点に作用させ、変形した前記動作軌道モデルを前記移動体の動作軌道とする動作軌道生成手段と、
を備えることを特徴とする移動体軌道生成方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【公開番号】特開２０１０−１５２６８４（Ｐ２０１０−１５２６８４Ａ）
【公開日】平成２２年７月８日（２０１０．７．８）
【国際特許分類】
【出願番号】特願２００８−３３０６６０（Ｐ２００８−３３０６６０）
【出願日】平成２０年１２月２５日（２００８．１２．２５）
【出願人】（０００００３０７８）株式会社東芝 (54,554)
【Ｆターム（参考）】
【公開日】平成２２年７月８日（２０１０．７．８）
【国際特許分類】
【出願日】平成２０年１２月２５日（２００８．１２．２５）
【出願人】（０００００３０７８）株式会社東芝 (54,554)
【Ｆターム（参考）】
[ Back to top ]