JP7222803B2 - 軌道計画装置、軌道計画方法及びプログラム - Google Patents

Description

本発明は、多軸ロボットアームの軌道計画技術に関するものである。

本技術分野の背景技術として、特許文献１が知られている。この特許文献１には、「しかしながら、従来例の場合には、経由教示点を通過する際に動作方向が急激に変化することになるため、その経由教示点で大きな加速度が発生することがある。その結果、振動が引き起こされ、所要の精度が得られなかったり、無理な力が加わることによってウェハやガラス基板等の搬送対象や機械本体の破損を招いたりするという問題の生じるおそれがあった。本発明は、上記課題を解決するためになされたものであり、外乱を考慮して設定された経由教示点に応じてマニピュレータを移動させる場合であっても、速度変化を滑らかにすることができる制御装置等を提供することを目的とする。」と記載されている。

特開２０１４－１０４５５８号公報

前記特許文献１には、開始教示点、目標教示点、経由教示点を入力とし、前記開始教示点、経由教示点、目標教示点間をそれぞれ結んだ線分上に定義される曲線補間点を通るように、曲線補間点を結ぶ軌道を算出する手法が記載されている。しかし、特許文献１の制御手法を多軸ロボットアームに適用する場合、すべての教示点を各軸の関節角度値、または手先の姿勢で入力する必要があるため、以下の問題が生じる。

すべての教示点を各軸の関節角度で指定する場合、確かに関節角度空間上では滑らかに接続されるが、同じ手先姿勢を実現する複数の関節角度のうちいずれか一つをユーザが入力する必要がある。このため、入力する教示点によっては、軌道上で手先姿勢が大きく変化する可能性が生じる、という問題がある。

一方で、すべての教示点を手先姿勢で決める場合、手先は滑らかに移動するものの、曲線補間点付近で、各軸の関節角度の変化が大きくなる可能性があるため、ロボットアームの各軸の角速度や角加速度の制約上、ロボットアームの動作が遅くなるという問題がある。つまり、特許文献１の制御手法では、手先姿勢の変化を最小にしつつ、ロボットアームが高速に動作する軌道が得られない場合があった。

そこで本発明は、経由点、及び目標点を手先姿勢で入力することで、手先姿勢の滑らかな移動を実現し、経路計画中に関節角度空間での平滑化処理を導入することで、関節角度変化を滑らかにして、ロボットアームの手先姿勢の変化を最小化しつつ、ロボットアームが高速に動作する軌道を出力する装置を提供する。

本発明は、プロセッサとメモリを有して、多軸で構成されたロボットアームの手先の軌道を算出する軌道計画装置であって、ロボットアームの構成と、ロボットアームの関節を構成する軸の位置及び軸の姿勢を含むロボットアーム構成情報と、計画する軌道の開始姿勢における前記ロボットアームの各軸の角度を開始関節角度として設定された開始関節角度情報と、前記ロボットアームの手先の終点で、目標位置と、前記ロボットアームの手先の目標姿勢が設定された目標姿勢情報と、計画する軌道内で前記ロボットアームの手先が通過する位置と姿勢を含む経由点が設定された経由点姿勢情報と、前記ロボットアーム構成情報と、前記開始関節角度情報と、前記目標姿勢情報と、前記経由点姿勢情報を読み込んで、ロボットアームの手先の開始点から終点までの軌道を、前記経由点間を補間して生成する物理空間経由点間軌道計画部と、前記ロボットアーム構成情報に基づいて、前記ロボットアームの手先の姿勢及び位置から各軸の関節角度を算出する逆運動学部と、前記物理空間経由点間軌道計画部で生成された前記軌道を、前記逆運動学部で関節角度空間に変換してから当該軌道を平滑化する関節角度空間軌道平滑化部と、を有する。

本発明によれば、手先姿勢の変化を最小化しつつ、複数の経由点を通りロボットアームが高速に移動できる軌道を算出する装置を提供することができる。

上記した以外の課題、構成及び効果は、以下の実施形態の説明により明らかにされる。

本発明の実施例を示し、多軸ロボットアームの軌道計画装置の構成の一例を示すブロック図である。 本発明の実施例を示し、軌道計画装置で行われる処理の一例を示すフローチャートである。 本発明の実施例を示し、ロボットアーム構成データの一例を示す図である。 本発明の実施例を示し、干渉物構成データの一例を示す図である。 本発明の実施例を示し、開始関節角度データの一例を示す図である。 本発明の実施例を示し、目標姿勢データの一例を示す図である。 本発明の実施例を示し、経由点姿勢データの一例を示す図である。 本発明の実施例を示し、軌道補間方法データの一例を示す図である。 本発明の実施例を示し、軌道データの一例を示す図である。 本発明の実施例を示し、図２のステップＳ１０３で行われる、物理空間経由点間軌道計画部が実行する処理の一例を示すフローチャートである。 本発明の実施例を示し、図２のステップＳ１０４で行われる、関節角度空間軌道平滑化部が実行する処理の一例を示すフローチャートである。 本発明の実施例を示し、出力画面の一例を示す図である。 本発明の実施例を示し、ロボットアームの軌道の一例を示す図である。 本発明の実施例を示し、部分軌道で軌道計画を実施した結果の一例を示す図である。 本発明の実施例を示し、平滑化可否の情報を用いた関節角度空間上の軌道の一例を示す図である。 本発明の実施例を示し、物理空間での手先の軌道の一例を示す図である。 本発明の実施例を示し、関節角度空間で平滑化を行った手先の軌道の一例を示す図である。

以下、図面を用いて実施例を説明する。なお、実施の形態を説明するための全図において、同一部には原則として同一符号を付し、その繰り返しの説明は省略する。本実施例では、本発明の基本の形態となる軌道計画装置の一例を説明する。

＜システム構成＞
図１は、本発明が適用される計算機システムを示し、ロボットアームの軌道計画装置１００及び周辺機器を含む構成例を示すブロック図である。計算機システム全体は、軌道計画装置１００と、入出力装置１４０を有する。ユーザは、入出力装置１４０の操作を通じて軌道計画装置１００の機能を利用する。軌道計画装置１００は、計算機（ＰＣ、サーバ等）で構成可能であり、例えばソフトウェアプログラム処理により本発明の特徴である機能（処理装置１１０の各処理部）を実現する。

軌道計画装置１００は処理装置１１０と、記憶装置１２０と、入出力インタフェース１３０などを有する。

入出力装置１４０は、ユーザの操作により、計測データなどの入力を行う入力装置や基準形状引当結果などの出力を行う出力装置であり、例えばキーボード、マウス、ディスプレイ、プリンタ、スマートフォン、タブレット型ＰＣなどがある。

入出力インタフェース１３０は、入出力装置１４０とデータ交換などのインタフェース制御（周辺デバイス制御）の処理を行う構成要素である。本計算機システムでは、処理装置１１０の処理及び入出力インタフェース１３０の処理に基づいて、入出力装置１４０の画面で、グラフィカルユーザインタフェース（ＧＵＩ）を提供し、各種の情報を表示する。

処理装置１１０は、例えばＣＰＵ３０、ＲＡＭ１０、ＲＯＭ２０等の公知または周知の要素により構成される。処理装置１１０は、本発明の特徴的な機能を実現する処理を行う構成要素であり、データ読込部２０１と、干渉判定部２０２と、逆運動学部２０３と、物理空間経由点間軌道計画部２０４と、関節角度空間軌道平滑化部２０５と、結果出力部２０６と、制御時刻付与部２０７を有する

なお、軌道計画装置１００は、図示はしないが、ＯＳ、ミドルウェア、アプリケーションなどの公知の要素を有し、特にディスプレイなどの入出力装置１４０にＧＵＩ画面を表示するための既存の処理機能を含む。処理装置１１０は、上記の処理機能を用いて、所定の画面を描画して表示する処理や、画面上でユーザから入力されるデータの処理などを行う。

データ読込部２０１と、干渉判定部２０２と、逆運動学部２０３と、物理空間経由点間軌道計画部２０４と、関節角度空間軌道平滑化部２０５と、結果出力部２０６の各機能部はプログラムとしてＲＡＭ１０にロードされる。

ＣＰＵ３０は、各機能部のプログラムに従って処理を実行することによって、所定の機能を提供する機能部として稼働する。例えば、ＣＰＵ３０は、干渉判定プログラムに従って処理を実行することで干渉判定部２０２として機能する。他のプログラムについても同様である。さらに、ＣＰＵ３０は、各プログラムが実行する複数の処理のそれぞれの機能を提供する機能部としても稼働する。計算機及び計算機システムは、これらの機能部を含む装置及びシステムである。

記憶装置１２０は、例えばＨＤＤやＳＳＤ等の公知または周知の不揮発性記憶媒体により構成され、ロボットアーム構成記憶部３０１と、干渉物構成記憶部３０２と、開始関節角度記憶部３０３と、目標姿勢記憶部３０４と、経由点姿勢記憶部３０５と、軌道補間方法記憶部３０６と、軌道記憶部３０７を含む。各記憶部は、例えば、データベースやテーブルを有する。

ロボットアーム構成記憶部３０１は、軌道計画や逆運動学などで用いるロボットアーム構成データ４０１を記憶する領域である。

干渉物構成記憶部３０２は、軌道計画で行う干渉判定に用いる干渉物構成データ４０２を記憶する領域である。

開始関節角度記憶部３０３は、軌道計画での開始点となる開始関節角度データ４０３を記憶する領域である。なお、関節角度は、ロボットアームの関節の軸回りの角度を示す。または、軸に連結された一対のリンクがなす角度としてもよい。

目標姿勢記憶部３０４は、軌道計画での終了点となる目標姿勢データ４０４を記憶する領域である。

経由点姿勢記憶部３０５は、軌道計画での経由点となる経由点姿勢データ４０５を記憶する領域である。

軌道補間方法記憶部３０６は、各経由点間の軌道を計画するときに用いる軌道補間方法データ４０６を記憶する領域である。

軌道記憶部３０７は、結果出力部２０６で出力される軌道データ４０７を記憶する領域である。

なお、本実施例では、ロボットアームの構造や形状については特定しないが、例えば、６軸のロボットアームで、手先（先端）のマニピュレータで対象物を把持する例を想定する。また、ロボットアームは多軸であればよく、各軸（関節）は直列に配置され、各軸間はリンクで接続される。また、ロボットアームの先端はマニピュレータに限定されるものではなく、ドライバや溶接装置などの工具や機械で構成することができる。

また、本実施例では、軌道や経由点は、ロボットアームの手先の位置と姿勢で構成される。ロボットアームの手先の位置は、ロボットアームを構成する複数の軸のうち先端の軸の位置を示す。ロボットアームの手先の姿勢は、ロボットアームを構成する複数の軸のうち先端の軸の角度（関節角度）を示す。以下の説明では、ロボットアームの位置や姿勢という記述は、ロボットアームの手先の位置と姿勢を示す。

＜フローチャート＞
図２は、軌道計画処理の一例を示すフローチャートである。このフローチャートの各ステップの内容を、図１３、図１４、図１５、図１６を用いて説明する。なお、以下の説明では、各機能部を処理の主体として説明するが、上述のようにＣＰＵ３０を処理の主体としてもよい。この処理は、例えば、軌道計画装置１００のユーザが入出力装置１４０から所定の指令を入力した場合に開始される。

ステップＳ１０１ではデータ読込部２０１が、ユーザが入出力装置１４０にて入力する情報を基に、ロボットアーム構成記憶部３０１にロボットアーム構成データ４０１を格納し、干渉物構成記憶部３０２に干渉物構成データ４０２を格納し、開始関節角度記憶部３０３に開始関節角度データ４０３を格納し、目標姿勢記憶部３０４に目標姿勢データ４０４を格納し、経由点姿勢記憶部３０５に経由点姿勢データ４０５を格納し、軌道補間方法記憶部３０６に軌道補間方法データ４０６をそれぞれ格納する。

なお、本実施例では前述のデータをユーザが入出力装置１４０を経由して入力することを想定しているが、過去の入力データをはじめとした、すでに生成済みのデータを処理装置１１０へ読み込ませるようにしてもよい。

本ステップにおける入力データに関して図１３を基に説明する。図１３は、入力データの一例を表した図であり、ロボットアームが稼働する実空間（物理空間）のうちＸ－Ｙ平面での軌道を示す図である。

Ｘ－Ｙ平面において、開始関節角度データとしてＰ１が設定され、目標姿勢データとしてＰ５が設定される。Ｐ１からＰ５へ向かう途中の経由点姿勢データとしてＰ２、Ｐ３、Ｐ４が設定される。

また、軌道補間方法として、（Ｐ１、Ｐ２、直線（Ｓｔｒａｉｇｈｔ）、平滑化しない（Ｓｍｏｏｔｈｉｎｇ＝Ｆａｌｓｅ））、（Ｐ２、Ｐ３、曲線、平滑化する）、（Ｐ３、Ｐ４、曲線、平滑化する）、（Ｐ４、Ｐ５、直線、平滑化しない）という情報が与えられた場合を想定している。この例では、ロボットアームの手先が、Ｐ１からＰ２、Ｐ３、Ｐ４を経由してＰ５まで移動する場合の軌道計画をすることが目標である。

なお、図示の例では、点Ｐ１から点Ｐ２の区間の軌道を部分軌道Ｔ１として扱い、他の経由点間についても同様に部分軌道Ｔ２～Ｔ４とする。なお、本実施例では、経由点姿勢データ４０５で隣り合う姿勢ＩＤ４５１をひとつの部分軌道Ｔｉとして扱う。

ステップＳ１０２では、物理空間経由点間軌道計画部２０４が、軌道記憶部３０７に格納されている軌道データ４０７を取得し、格納されている軌道がすでに目標姿勢データ４０４に到達しているかを判定する。物理空間経由点間軌道計画部２０４は、すべての経由点間を通る軌道が算出されていればステップＳ１０４へ進み、未処理の区間があればステップＳ１０３へ進む。

ステップＳ１０３では、物理空間経由点間軌道計画部２０４が、記憶装置１２０に格納されている必要なデータを用いて、軌道データ４０７の形式に従って部分軌道を算出し、軌道記憶部３０７に格納されている軌道データ４０７に追記する。なお、本ステップＳ１０３の詳細は後述する。

本ステップＳ１０３での出力に関して、図１４を用いて説明する。図１４は、図１３で示した入力に対して、それぞれの部分軌道で軌道計画を実施した結果をロボットアームの関節角度空間で表示した結果である。

例えば、部分軌道Ｔ１（Ｐ１、Ｐ２）に対しては、図１３に示した物理空間上で直線軌道（Ｓｔｒａｉｇｈｔ）が割当てられており、その直線軌道を実現するために、関節角度空間ではθ１、θ２を接続する曲線のような軌道になる。

他の部分軌道に関しても同様であり、多くのロボットアームでは、物理空間で手先の軌道が曲線や直線であっても、関節角度空間では同じようにはならない。また、物理空間上では滑らかな線上を手先が動いている場合でも、関節角度空間上では経由姿勢などで滑らかに接続されない場合もある。

図１４では、関節角度空間のうち軸Ｊ１を横軸に配置し、軸Ｊ２を縦軸に配置した例を示す。関節角度空間は、ロボットアームの軸の数に応じた多次元空間であり、例えば、６軸のロボットアームの場合、軸Ｊ１～軸Ｊ６の６次元で表現される。

ステップＳ１０４では、関節角度空間軌道平滑化部２０５が、軌道記憶部３０７に格納されている軌道データ４０７に対して、関節角度空間において、平滑化を適用することで、軌道データ４０７を修正する。なお、本ステップＳ１０４の詳細は後述する。

本ステップＳ１０４での処理の概要に関して、図１５、図１６を用いて説明する。図１４に示した通り、軌道を複数接続した場合、関節角度空間上では滑らかに接続されない場合がある。本ステップでは、これらを滑らかに接続する処理を実施する。

図１５は、平滑化可否の情報を用いて、平滑化を施してよい軌道を選択し、関節角度空間上で滑らかに接続されるように補正された軌道である。図１５は、上記図１４と同様に、関節角度空間のうち軸Ｊ１、軸Ｊ２を表した図である。本処理により、複数の部分軌道Ｔ１～Ｔ４で構成される軌道は、経由点（Ｐ２～Ｐ４）を通過して、関節角度空間上で滑らかに接続される。

図１６は、図１５の関節角度空間上での軌道から、物理空間での手先の軌道を計算した図である。図１６の実線に示す通り、本処理により、入力された経由点を通りつつ、物理空間上でも滑らかに接続された軌道が得られる。

ステップＳ１０５では、制御時刻付与部２０７が、軌道データ４０７に対して、制御時刻４７６を各軌道点に対してそれぞれ付与する。また、制御時刻付与部２０７は、制御時刻４７６を付与した軌道データ４０７に対して、制御時刻４７６に応じた関節角速度情報４７３と関節角加速度情報４７４を算出する。この制御時刻４７６を付与するためには、ロボットアームの各関節の速度や角速度の制約を満たして行う必要があるが、この方法の一例として次の文献に記載された技術を適用することができる。「Time-Optimal Parabolic Interpolation with Velocity, Acceleration, and Minimum-Switch-Time Constraints」、Puttichai Lertkultanon and Quang-Cuong Pham、Published online: 16 Jul 2016。

ステップＳ１０６では、結果出力部２０６が、記憶装置１２０に格納されているデータを基にＧＵＩ（Ｇｒａｐｈｉｃａｌ Ｕｓｅｒ Ｉｎｔｅｒｆａｃｅ）を生成し、入出力装置１４０に表示する。

上記処理によって、関節角度空間軌道平滑化部２０５が経由点間の部分軌道に対して関節角度空間での平滑化処理を導入することで、ロボットアームの手先姿勢の変化を最小化しつつ、ロボットアームが高速に動作可能な軌道を生成することが可能となる。

＜ロボットアーム構成データ＞
図３は、ロボットアーム構成記憶部３０１に記憶するロボットアーム構成データ４０１の一例を示す図である。ロボットアーム構成データ４０１は、分類４１１と、項目４１２と、例４１３からひとつのエントリが構成される。分類４１１は、関節情報と、リンク情報の分類が含まれる。

関節情報とは、ロボットアームを構成する各関節の情報であり、項目４１２として関節名、関節の種類、関節の位置、関節の向き、関節の動作下限、関節の動作上限、最大加速度、最大速度などの情報が含まれる。

リンク情報とは、ロボットアームを構成するリンクの構成を表す情報であり、項目４１２としてリンク名、親関節名、子関節名、リンク形状が含まれる。リンク形状とは、リンクの実際の形状であり、例えば、ＳＴＥＰ（Standard for the Exchange of Product model data）などのフォーマットで保存されるソリッドデータや、ＳＴＬ（STereoLithography）などのフォーマットで保存されるポリゴンデータなどの３次元モデルデータである。

なお、例４１３には、各項目４１２対応する値が設定される。ロボットアーム構成データ４０１は、ロボットアームの製造元などによって予め設定されたデータを用いることができる。

＜干渉物構成データ＞
図４は、干渉物構成記憶部３０２に格納される干渉物構成データ４０２の一例を示す図である。干渉物構成データ４０２は、干渉物ＩＤ４２１と、干渉物形状４２２と、干渉物姿勢４２３をひとつのエントリに含む。

干渉物形状４２２は、干渉物の形状を示すものであり、例えばＳＴＥＰなどのフォーマットで保存されるソリッドデータや、ＳＴＬなどのフォーマットで保存されるポリゴンデータなどの３次元モデルデータである。

干渉物姿勢４２３は、干渉物が空間上のどの位置に置かれるかを示す情報であり、ＡＦＦＩＮＥ変換行列や、３次元空間上での位置と、Ｒｏｌｌ―Ｐｉｔｃｈ－Ｙａｗなどで表される姿勢を示す情報である。例えば、干渉物ＩＤ４２１＝「ＣＯＬ１」の干渉物姿勢４２３は、（０，０，１）が位置を示し、（０，０，０）が姿勢を示す。

＜開始関節角度データ＞
図５は、開始関節角度記憶部３０３に格納される開始関節角度データ４０３の一例を示す図である。開始関節角度データ４０３は、関節名４３１と、開始関節角度４３２をひとつのエントリに含む。

関節名４３１は、ロボットアーム構成データ４０１に含まれる関節情報内の関節名と対応するものである。開始関節角度４３２には、関節の角度が設定される。

＜目標姿勢データ＞
図６は、目標姿勢記憶部３０４に格納される目標姿勢データ４０４の一例を示す図である。目標姿勢データ４０４は、姿勢ＩＤ４４１と、対象リンク名４４２と、姿勢情報４４３とをひとつのエントリに含む。

姿勢ＩＤ４４１には、ロボットアームの手先の位置及び姿勢を特定する識別子が設定される。対象リンク名４４２には、ロボットアーム構成データ４０１に含まれるリンク情報内のリンク名が格納される。

また、姿勢情報４４３は、ＡＦＦＩＮＥ変換行列や、３次元空間上での位置及びＲｏｌｌ―Ｐｉｔｃｈ－Ｙａｗなどで表される姿勢を示す情報である。例えば、姿勢ＩＤ４４１＝「ＰＯＳＥ００１」の姿勢情報４４３は、（０，０，１）が位置を示し、（０，０，０）が姿勢を示す。

＜経由点姿勢データ＞
図７は、経由点姿勢記憶部３０５に格納される経由点姿勢データ４０５の一例を示す図である。経由点姿勢データ４０５は、姿勢ＩＤ４５１と、対象リンク名４５２と、姿勢情報４５３とをひとつのエントリに含む。

姿勢ＩＤ４５１には、ロボットアームの手先の位置及び姿勢を特定する識別子が設定される。対象リンク名４５２は、ロボットアーム構成データ４０１に含まれるリンク情報内のリンク名が格納される。また、姿勢情報は、ＡＦＦＩＮＥ変換行列や、３次元空間上での位置とそのときのＲｏｌｌ―Ｐｉｔｃｈ－Ｙａｗなどで表される姿勢を示す情報である。例えば、姿勢ＩＤ４５１＝「ＰＯＳＥ００２」の姿勢情報４５３は、（０，０，１）が位置を示し、（０，０，０）が姿勢を示す。

＜軌道補間方法データ＞
図８は、軌道補間方法記憶部３０６に格納される軌道補間方法データ４０６の一例を示す図である。軌道補間方法データ４０６は、開始姿勢ＩＤ４６１と、終了姿勢ＩＤ４６２と、軌道算出方法４６３と、平滑化可否４６４をひとつのエントリに含む。

開始姿勢ＩＤ４６１、及び終了姿勢ＩＤ４６２は、経由点姿勢データ４０５の姿勢ＩＤ４５１や、目標姿勢データ４０４の姿勢ＩＤ４４１に対応する。

軌道算出方法４６３は、開始姿勢ＩＤ４６１から及び終了姿勢ＩＤ４６２までの部分軌道Ｔで、関節角度空間で軌道を算出（補間）する方法が格納される。算出方法としては、例えば、スプライン曲線、ＮＵＲＢＳ曲線、ベジエ曲線といった曲線パターンや、曲線を表す数式や直線を指定する記述が含まれる。

平滑化可否４６４は、開始点（開始姿勢ＩＤ４６１）から終了点（終了姿勢ＩＤ４６２）までの部分軌道を、関節角度で平滑化するか否かを示すフラグである。

軌道補間方法データ４０６は、経由点姿勢データ４０５で指定された経由点間で構成される部分軌道Ｔｉについて、関節角度空間で軌道を算出する方法（４６３）と、関節角度空間での円滑化を実施するか否かを示す情報（４６４）を予め設定したテーブルである。

＜軌道データ＞
図９は、軌道記憶部３０７に格納される軌道データ４０７の一例を示す図であり、軌道計画装置１００の出力に相当する。軌道データ４０７は、軌道点ＩＤ４７１と、関節角度情報４７２と、関節角速度情報４７３と、関節角加速度情報４７４と、平滑化可否４７５と、制御時刻４７６をひとつのエントリに含む。

軌道点ＩＤ４７１は、ロボットアームの手先が通過する位置の識別子が格納される。関節角度情報４７２は、軌道点ＩＤ４７１の位置における関節角度の集合であり、関節名とその関節値の集合である。

関節角速度情報４７３、関節角加速度情報４７４も同様に、関節名と軌道点ＩＤ４７１の位置における角速度、角加速度の情報である。平滑化可否４７５は、関節角度空間軌道平滑化部２０５が利用する情報であり、関節角度で平滑化を実行して関節角度情報を補正してよいのか否かを示すフラグである。「ＴＲＵＥ」が平滑化可能を示し、「ＦＡＬＳＥ」が平滑化不可を示す。

制御時刻４７６は、ロボットアームの速度の制約や、加速度の制約に基づいて算出された軌道点ＩＤ４７１の制御を実施する時刻（相対的な時刻）が格納される。

＜部分軌道算出処理＞
図１０は、図２のステップＳ１０３で行われる、物理空間経由点間軌道計画部２０４が実行する処理の一例を示すフローチャートである。以下、本フローチャートを説明する。

ステップＳ２０１では、物理空間経由点間軌道計画部２０４が、開始関節角度、及び終了姿勢を取得する。開始関節角度は、軌道データ４０７に含まれている最後に追加された軌道点ＩＤ４７１に対応する関節角度情報４７２より取得する。

また、軌道計算に必要な軌道の開始位置や終了位置などのデータは、上記ステップＳ１０１で入力されたデータを用いるものとする。終了姿勢は、目標姿勢データ４０４の姿勢情報４４３から設定される。

ステップＳ２０２では、物理空間経由点間軌道計画部２０４が、終了姿勢ＩＤ４６２に対応する軌道補間方法データ４０６から、軌道算出方法４６３を取得する。

ステップＳ２０３では、物理空間経由点間軌道計画部２０４が、経由点姿勢データ４０５で対象とする対象リンク名４５２の、開始関節角度データ４０３における開始姿勢Ｐ１を計算する。この開始姿勢Ｐ１は、ロボットアーム構成データ４０１の情報を用いて、順運動学により容易に算出できる。

ステップＳ２０４では、物理空間経由点間軌道計画部２０４が、開始姿勢Ｐ１から、終了姿勢Ｐｎまでを、取得した軌道算出方法４６３に従って開始姿勢ＩＤ４６１と終了姿勢ＩＤ４６２をひとつの部分軌道として分割する。

説明の便宜上、姿勢情報の位置に関する情報をＯ、姿勢に関する情報をＱとする。例えば、軌道算出方法４６３として直線（Ｓｔｒａｉｇｈｔ）が指定された場合、物理空間経由点間軌道計画部２０４は、開始姿勢Ｐ１から終了姿勢Ｐｎまでの各分割点Ｐｉに対応するОｉ、Ｑｉは以下のように算出する。

ここで、Ｓｌｅｒｐとは球面補間を意味し、ｔは０から１の等間隔の値を意味する。 また、軌道算出方法４６３として３次のベジエ曲線（ＢＥＺＩＥＲ）が指定された場合、二つの制御点を、Ａ、Ｂとすると、各分割点Ｐｉに対応するОｉ、Ｑｉは以下のように算出する。

物理空間経由点間軌道計画部２０４は、上記の式を用いることで、手先姿勢の変化を最小にしつつ、物理空間上を指定された方法の通りに動く軌道が得られる。

ステップＳ２０５では、物理空間経由点間軌道計画部２０４が、ステップＳ２０６からＳ２１０までの処理を、分割点の数だけループして実行する。

ステップＳ２０６では、物理空間経由点間軌道計画部２０４が、軌道データ４０７に含まれている最後に追加された軌道点ＩＤ４７１に対応する関節角度情報４７２を取得し、この関節角度を始点として、分割点姿勢Ｐｉ＋１になる関節角度θｉ＋１を、逆運動学部２０３を用いて計算する。

逆運動学部２０３で行う処理は、以下の文献のような収束計算による方法を一例として用いることができる。「Solvability-unconcerned Inverse Kinematics based on Levenberg-Marquardt method with Robust Damping」（Tomomichi Sugihara、School of Information Science and Electrical Engineering, Kyushu University）

ステップＳ２０７では、物理空間経由点間軌道計画部２０４が、上記ステップＳ２０６で得られた関節角度θｉ＋１と、ロボットアーム構成データ４０１と、干渉物構成データ４０２を用いて、計算された関節角度θｉ＋１において干渉物との接触が無いかを干渉判定部２０２により判定する。

干渉判定部２０２は、ロボットアーム構成データ４０１の３次元モデルデータと、ロボットアームの手先の位置及び姿勢と、干渉物構成データ４０２の３次元モデルデータと位置に基づいて干渉の有無を判定する。なお、干渉の判定については周知または公知の技術を適用すればよいので、本実施例では詳述しない。

ここで、干渉判定部２０２が行う処理は、障害物とロボットアームが接触するかしないかの判定だけでなく、障害物とロボットアームの距離を基に所定のクリアランスを閾値とし、クリアランスが閾値未満であれば干渉ありと判定してもよい。

ステップＳ２０８では、物理空間経由点間軌道計画部２０４が、上記ステップＳ２０７での干渉判定結果を基に処理の分岐を行い、干渉が無ければステップＳ２０９へ進み、干渉があればステップＳ２１１へ進む。

ステップＳ２０９では、物理空間経由点間軌道計画部２０４が、上記ステップＳ２０７で得られた関節情報θｉ＋１を、軌道データ４０７に対して、軌道点ＩＤ４７１、関節角度情報４７２、平滑化可否４７５として追加する。この場合、平滑化可否４７５は、軌道補間方法データ４０６に含まれる平滑化可否４６４を基に設定される。また、制御時刻４７６は、上記ステップＳ１０５の処理で設定される。

なお、本実施例の軌道データ４０７では、関節角度情報４７２等は６軸のロボットアームの例を示すが、これに限定されるものでなはく、ロボットアームの軸数に応じた次元とすればよい。

ステップＳ２１１では、物理空間経由点間軌道計画部２０４が、干渉があった場合、軌道計画に失敗したことを示す情報を入出力装置１４０に表示して処理を終了する。

上記処理により、指定された開始位置から終了位置までの軌道は、開始姿勢Ｐ１～終了姿勢Ｐｎまでの部分軌道に分割されて、部分軌道ごとに指定された軌道算出方法４６３で軌道データ４０７が算出されて、軌道点ＩＤ４７１毎に物理空間における関節角度θｉが決定される。

＜軌道平滑化処理＞
図１１は、図２のステップＳ１０４で関節角度空間軌道平滑化部２０５が実行する、軌道平滑化処理の一例を示すフローチャートである。

本処理により、ロボットアームが円滑に作動するよう軌道データ４０７内の関節角度情報４７２が更新されることで、ロボットアームの各軸での角度変化が小さくなる。これにより、制御時刻付与部２０７が行う制御時刻付与処理において、関節角速度や関節角加速度の制約の順守が容易になり、ロボットアームの動作速度が向上する。

また、部分軌道が接続する点Ｐｉ（経由点）において、関節角速度や関節角加速度の変化（差分）が一致または最小化されて、部分軌道間の繋ぎ目が滑らかになる効果を有する。なお、関節角度空間軌道平滑化部２０５は、隣り合う部分軌道Ｔｉを接続する経由点（Ｐｉ）で、隣接する部分軌道の角速度又は角加速度を一致させるようにしてもよい。

以下、本フローチャートを説明する。

ステップＳ３０１では、関節角度空間軌道平滑化部２０５が、軌道内の各軌道点についてステップＳ３０２の処理を、平滑化が完了するまでループ処理をする。このループ処理は、軌道内の各軌道点の処理が完了するまでステップＳ３１３との間で繰り返す。

ステップＳ３０２では、関節角度空間軌道平滑化部２０５が、軌道データ４０７から、平滑化可否４７５を基に、平滑化不可に挟まれる平滑化可能な軌道点の集合を取得する。

ステップＳ３０３では、関節角度空間軌道平滑化部２０５が、ステップＳ３０４からステップＳ３１２までの処理に関して、上記ステップＳ３０２で取得した平滑化不可に挟まれる平滑化可能な軌道点の集合の数だけループ処理を行う。

ステップＳ３０４では、関節角度空間軌道平滑化部２０５が、平滑化可能な軌道点を挟む平滑化不可である軌道点の情報を基に、平滑化対象の軌道点間について関節角度空間上での補間曲線Ｓを計算する。

本ステップを図１７を用いて説明する。図１７は、関節角度空間で平滑化を行った手先の軌道点の一例を示す図である。平滑化対象の軌道点における関節角度をθ－２～θ～θ＋２とする。図中補間曲線Ｓが、平滑化対象の軌道点間について関節角度空間上で算出された曲線である。

平滑化対象の部分の前後にある平滑化不可な軌道点の関節角度をθａ、θｂとする。この関節角度値と、その前後の関節角度θａ－１、θｂ＋１の値から、以下の制約を満たす補間方程式Ｆ（ｔ）を計算する。

上記の例は、区間［θａ－１、θａ］と、［θｂ、θｂ＋１］を直線とした場合の補間曲線における制約である。この補間曲線の求め方は一例であり、例えば、すべての平滑化不可の軌道点に対応する関節角度値を通る３次元のスプライン補間曲線のうち、この区間に該当するものなどでもよい。

ステップＳ３０５では、関節角度空間軌道平滑化部２０５が、ステップＳ３０３で対象とする部分に含まれる平滑化可能な軌道点の数だけループ処理を行う。このループ処理は、ステップＳ３１１までの間で繰り返される。

ステップＳ３０６では、関節角度空間軌道平滑化部２０５が、軌道点（θｉ）を補間曲線Ｓ上に動かす方向ｄｉを計算し、ｄｉに向かって移動させた新たな軌道点（θ’ｉ）を計算する。なお、以下の説明では、図１７の関節角度θｉに対応する軌道点を軌道点（θｉ）で表す。

軌道点（θｉ）及び軌道点（θ’ｉ）の計算結果は、図１７に示した通りになる。このステップで一度に軌道点（θ’ｉ）を補間曲線Ｓ上に移動させた場合、移動先で干渉物との干渉があると平滑化されない。そのため、少しずつ移動させていくことが必要である。すなわち、関節角度空間軌道平滑化部２０５は、軌道点（θｉ）から所定の距離ずつ方向ｄｉへ軌道点（θ’ｉ）を移動させ、最終的に補間曲線Ｓ上の軌道点（θ’ｉ）へ移動させる。

ステップＳ３０７では、関節角度空間軌道平滑化部２０５が、干渉判定部２０２を用いて、更新予定である関節角度（θ’ｉ）の位置で、ロボットアームが干渉物との干渉が無いかを判定する。

ステップＳ３０８では、関節角度空間軌道平滑化部２０５が、干渉の有無により処理を分岐する。干渉が無ければステップＳ３０９へ進み、干渉があればステップＳ３１０に進む。

ステップＳ３０９では、関節角度空間軌道平滑化部２０５が、該当する軌道点の関節角度θｉをθ’ｉへ更新する。

一方、ステップＳ３１０では、関節角度空間軌道平滑化部２０５は、干渉が発生したので、現在の軌道点に対する平滑化可否４７５を「ＦＡＬＳＥ」へ変更する。つまり、ロボットアームを補間曲線Ｓ上で動かすと干渉するような軌道点が新たに固定点となるため、以降のステップＳ３０４で行われる補間曲線が変化するようになる。

上記処理によって、図１７で示したように、平滑化不可の軌道点（θａ、θｂ）に挟まれる平滑化可能な軌道点（θｉ）は、干渉物との干渉を判定しながら徐々に補間曲線Ｓ上の軌道点（θ’ｉ）に移動し、関節角度空間上で滑らかな軌道を得ることができる。

このように、物理空間経由点間軌道計画部２０４で生成した軌道を、逆運動学部２０３で関節角度空間に変換した軌道に平滑化を実施することで、ロボットアームの各軸の角速度や角加速度の急変や過大な変動を抑制することが可能になる。これにより、ロボットアームの手先姿勢の変化を最小化しながらも、複数の経由点を高速に移動可能なロボットアームの軌道を算出することができる。

＜出力画面＞
図１２は、軌道計画装置１００の出力となる出力画面１０５の一例を示す図である。出力画面１０５には、入力データを読み込む読み込みボタン（図中「読込」）１０１と、軌道計算を実行させる計算開始ボタン（図中「計算開始」）１０２と、軌道の計算結果を示す表１０３と、ロボットアームの動きを表示するシミュレート領域１０４が含まれる。

読み込みボタン１０１をマウスなどで操作することにより、入出力インタフェース１３０を介して入力された入力データが、ＲＡＭ１０へ読み込まれる。計算開始ボタン１０２の操作により、軌道計画装置１００では軌道計画計算が実行され、シミュレート領域１０４と表１０３が生成される。

表１０３には、例えば、平滑化の処理回数と、平滑化による動作時間が表示される。また、表内の動作再生ボタンを操作することで、シミュレート領域１０４でロボットアームの動作を視認することができる。

以上に説明したように。本システム（多軸ロボットアームの軌道計画装置１００）によれば、指定された経由点を通過し、かつ、手先姿勢を滑らかに接続し、ロボットアームが円滑に動作しやすい軌道を生成することが可能となる。
＜結び＞

以上のように、上記実施例の軌道計画装置は以下の構成とすることができる。

（１）プロセッサ（ＣＰＵ３０）とメモリ（ＲＡＭ１０）を有して、多軸で構成されたロボットアームの手先の軌道を算出する軌道計画装置（１００）であって、ロボットアームの構成と、ロボットアームの関節を構成する軸の位置及び軸の姿勢を含むロボットアーム構成情報（ロボットアーム構成データ４０１）と、計画する軌道の開始姿勢における前記ロボットアームの各軸の角度を開始関節角度（４３２）として設定された開始関節角度情報（開始関節角度データ４０３）と、前記ロボットアームの手先の終点で、目標位置と、前記ロボットアームの手先の目標姿勢（姿勢情報４４３）が設定された目標姿勢情報（目標姿勢データ４０４）と、計画する軌道内で前記ロボットアームの手先が通過する位置と姿勢を含む経由点が設定された経由点姿勢情報（経由点姿勢データ４０５）と、前記ロボットアーム構成情報（４０１）と、前記開始関節角度情報（４０３）と、前記目標姿勢情報（４０４）と、前記経由点姿勢情報（４０５）を読み込んで、ロボットアームの手先の開始点から終点までの軌道を、前記経由点間を補間して生成する物理空間経由点間軌道計画部（２０４）と、前記ロボットアーム構成情報（４０１）に基づいて、前記ロボットアームの手先の姿勢及び位置から各軸の関節角度を算出する逆運動学部（２０３）と、前記物理空間経由点間軌道計画部（２０４）で生成された前記軌道を、前記逆運動学部（２０３）で関節角度空間に変換してから当該軌道を平滑化する関節角度空間軌道平滑化部（２０５）と、を有する。

上記構成により、軌道計画装置１００は、物理空間経由点間軌道計画部２０４で生成した軌道を、逆運動学部２０３で関節角度空間に変換した軌道に平滑化を実施することで、ロボットアームの各軸の角速度や角加速度の急変や過大な変動を抑制することが可能になる。これにより、ロボットアームの手先姿勢の変化を最小化しながらも、複数の経由点を高速に移動可能なロボットアームの軌道を算出することができる。

（２）上記（１）に記載の軌道計画装置であって、前記関節角度空間軌道平滑化部（２０５）で平滑化された軌道に対して、当該軌道の経由点を通過する時刻を付与する軌道時刻付与部（２０７）を、さらに有する。

上記構成により、ロボットアームの手先姿勢の変化が最小化されているため、軌道計画装置１００は、ロボットアームの速度（角速度）の制約や加速度（角加速度）の制約を満たした円滑な軌道を算出できる。

（３）上記（１）に記載の軌道計画装置（１００）であって、隣り合う経由点間を部分軌道として当該部分軌道における平滑化の可否を示す平滑化可否情報（４６４）と、当該平滑化を行う際の補間方法（４６３）を設定した軌道補間情報（軌道補間方法データ４０６）を、さらに有し、前記関節角度空間軌道平滑化部（２０５）は、前記軌道補間情報（４０６）を参照して、軌道を算出する対象の部分軌道の平滑化可否情報（４７５）が可であれば前記補間方法に基づいて前記軌道を補間する。

上記構成により、平滑化可否４７５が「可」の部分軌道では、指定された軌道算出方法４６３で関節角度空間での平滑化が行われる。これにより、軌道計画装置１００は、ロボットアームの手先姿勢の変化を最小化しながらも、複数の経由点を高速に移動可能なロボットアームの軌道を算出することができる。

（４）上記（１）に記載の軌道計画装置（１００）であって、前記ロボットアーム構成情報（４０１）は、ロボットアームの３次元モデル情報を含み、前記ロボットアームの周囲に配置された干渉物の３次元モデル情報及び位置を含む干渉物構成情報（４０２）と、前記物理空間経由点間軌道計画部（２０４）又は前記関節角度空間軌道平滑化部（２０５）で算出された軌道について、前記ロボットアーム構成情報（４０１）と前記干渉物構成情報（４０２）を参照して前記ロボットアームが前記干渉物と干渉するか否かを判定する干渉判定部（２０２）と、をさらに有する。

上記構成により、軌道計画装置１００は、ロボットアームが干渉物に干渉する軌道を回避して、円滑な軌道を算出することができる。

（５）上記（２）に記載の軌道計画装置（１００）であって、隣り合う経由点間を部分軌道として当該部分軌道における平滑化の可否を示す平滑化可否情報（４７５）と、当該平滑化を行う際の補間方法を設定した軌道補間情報（４０６）を、さらに有し、前記軌道時刻付与部（２０７）は、隣り合う前記部分軌道の経由点において、隣接する部分軌道の速度又は角加速度を一致させる。

上記構成により、軌道計画装置１００は、部分軌道が接続する点Ｐｉ（経由点）において、ロボットアームの角速度や角加速度の変化が一致または最小化されて、部分軌道間の繋ぎ目が滑らかになり、ロボットアームの円滑な動作を実現することが可能となる。

なお、本発した実施例に限定されるものではなく、様々な変形例が含まれる。例えば、上記した実施例は本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。また、ある実施例の構成の一部を他の実施例の構成に置き換えることが可能であり、また、ある実施例の構成に他の実施例の構成を加えることも可能である。また、各実施例の構成の一部について、他の構成の追加・削除・置換をすることが可能である。

また、上記の各構成、機能、処理部、処理手段等は、それらの一部又は全部を、例えば集積回路で設計する等によりハードウェアで実現してもよい。また、上記の各構成、機能等は、プロセッサがそれぞれの機能を実現するプログラムを解釈し、実行することによりソフトウェアで実現してもよい。各機能を実現するプログラム、テーブル、ファイル等の情報は、メモリや、ハードディスク、ＳＳＤ（Solid State Drive）等の記録装置、または、ＩＣカード、ＳＤカード、ＤＶＤ等の記録媒体に置くことができる。

また、制御線や情報線は説明上必要と考えられるものを示しており、製品上必ずしも全ての制御線や情報線を示しているとは限らない。実際には殆ど全ての構成が相互に接続されていると考えてもよい。

１００ 軌道計画装置
１１０ 処理装置
１２０ 記憶装置
１３０ 入出力インタフェース
１４０ 入出力装置
２０１ データ読込部
２０２ 干渉判定部
２０３ 逆運動学部
２０４ 物理空間経由点間軌道計画部
２０５ 関節角度空間軌道平滑化部
２０６ 結果出力部
２０７ 制御時刻付与部
３０１ ロボットアーム構成記憶部
３０２ 干渉物構成記憶部
３０３ 開始関節角度記憶部
３０４ 目標姿勢記憶部
３０５ 経由点姿勢記憶部
３０６ 軌道補間方法記憶部
３０７ 軌道記憶部

Claims (11)

1. プロセッサとメモリを有して、多軸で構成されたロボットアームの手先の軌道を算出する軌道計画装置であって、
   ロボットアームの構成と、ロボットアームの関節を構成する軸の位置及び軸の姿勢を含むロボットアーム構成情報と、
   計画する軌道の開始姿勢における前記ロボットアームの各軸の角度を開始関節角度として設定された開始関節角度情報と、
   前記ロボットアームの手先の終点で、目標位置と、前記ロボットアームの手先の目標姿勢が設定された目標姿勢情報と、
   計画する軌道内で前記ロボットアームの手先が通過する位置と姿勢を含む経由点が設定された経由点姿勢情報と、
   前記ロボットアーム構成情報と、前記開始関節角度情報と、前記目標姿勢情報と、前記経由点姿勢情報を読み込んで、ロボットアームの手先の開始点から前記終点までの軌道を、前記経由点間を補間して生成する物理空間経由点間軌道計画部と、
   前記ロボットアーム構成情報に基づいて、前記ロボットアームの手先の姿勢及び位置から各軸の関節角度を算出する逆運動学部と、
   前記物理空間経由点間軌道計画部で生成された前記軌道を、前記逆運動学部で関節角度空間に変換してから当該軌道を平滑化する関節角度空間軌道平滑化部と、
   を有することを特徴とする軌道計画装置。
2. 請求項１に記載の軌道計画装置であって、
   前記関節角度空間軌道平滑化部で平滑化された軌道に対して、当該軌道の経由点を通過する時刻を付与する軌道時刻付与部を、さらに有することを特徴とする軌道計画装置。
3. 請求項１に記載の軌道計画装置であって、
   隣り合う経由点間を部分軌道として当該部分軌道における平滑化の可否を示す平滑化可否情報と、当該平滑化を行う際の補間方法を設定した軌道補間情報を、さらに有し、
   前記関節角度空間軌道平滑化部は、
   前記軌道補間情報を参照して、軌道を算出する対象の部分軌道の平滑化可否情報が可であれば前記補間方法に基づいて前記軌道を補間することを特徴とする軌道計画装置。
4. 請求項１に記載の軌道計画装置であって、
   前記ロボットアーム構成情報は、ロボットアームの３次元モデル情報を含み、
   前記ロボットアームの周囲に配置された干渉物の３次元モデル情報及び位置を含む干渉物構成情報と、
   前記物理空間経由点間軌道計画部又は前記関節角度空間軌道平滑化部で算出された軌道について、前記ロボットアーム構成情報と前記干渉物構成情報を参照して前記ロボットアームが前記干渉物と干渉するか否かを判定する干渉判定部と、をさらに有することを特徴とする軌道計画装置。
5. 請求項２に記載の軌道計画装置であって、
   隣り合う経由点間を部分軌道として当該部分軌道における平滑化の可否を示す平滑化可否情報と、当該平滑化を行う際の補間方法を設定した軌道補間情報を、さらに有し、
   前記軌道時刻付与部は、
   隣り合う前記部分軌道の経由点において、隣接する部分軌道の速度又は加速度を一致させることを特徴とする軌道計画装置。
6. プロセッサとメモリを有する軌道計画装置が、多軸で構成されたロボットアームの手先の軌道を算出する軌道計画方法であって、
   前記軌道計画装置が、ロボットアームの構成と、ロボットアームの関節を構成する軸の位置及び軸の姿勢を含むロボットアーム構成情報と、計画する軌道の開始姿勢における前記ロボットアームの各軸の角度を開始関節角度として設定された開始関節角度情報と、前記ロボットアームの手先の終点で、目標位置と、前記ロボットアームの手先の目標姿勢が設定された目標姿勢情報と、計画する軌道内で前記ロボットアームの手先が通過する位置と姿勢を含む経由点が設定された経由点姿勢情報と、を読み込んで、前記ロボットアームの手先の開始点から前記終点までの軌道を、前記経由点間を補間して生成する物理空間軌道計画ステップと、
   前記軌道計画装置が、前記生成された軌道について、前記ロボットアーム構成情報を参照して前記ロボットアームの手先の姿勢及び位置から各軸の関節角度を算出し、前記軌道を関節角度空間に変換する逆運動学ステップと、
   前記軌道計画装置が、前記関節角度空間に変換され前記軌道を平滑化する関節角度空間平滑化ステップと、
   を含むことを特徴とする軌道計画方法。
7. 請求項６に記載の軌道計画方法であって、
   前記軌道計画装置が、前記関節角度空間平滑化ステップで平滑化された軌道に対して、当該軌道の経由点を通過する時刻を付与する軌道時刻付与ステップを、さらに含むことを特徴とする軌道計画方法。
8. 請求項６に記載の軌道計画方法であって、
   前記関節角度空間平滑化ステップは、
   隣り合う経由点間を部分軌道として当該部分軌道における平滑化の可否を示す平滑化可否情報と、当該平滑化を行う際の補間方法を設定した軌道補間情報を読み込んで、前記軌道を算出する対象の部分軌道の平滑化可否情報が可であれば前記補間方法に基づいて前記軌道を補間することを特徴とする軌道計画方法。
9. 請求項６に記載の軌道計画方法であって、
   前記ロボットアーム構成情報は、前記ロボットアームの３次元モデル情報を含み、
   前記軌道計画装置が、前記ロボットアームの周囲に配置された干渉物の３次元モデル情報及び位置を含む干渉物構成情報と、を読み込んで、前記物理空間軌道計画ステップ又は前記関節角度空間平滑化ステップで算出された軌道について、前記ロボットアーム構成情報と前記干渉物構成情報に基づいて前記ロボットアームが前記干渉物と干渉するか否かを判定する干渉判定ステップを、さらに含むことを特徴とする軌道計画方法。
10. 請求項７に記載の軌道計画方法であって、
    前記軌道時刻付与ステップは、
    隣り合う経由点間を部分軌道として当該部分軌道における平滑化の可否を示す平滑化可否情報と、当該平滑化を行う際の補間方法を設定した軌道補間情報を、読み込んで、隣り合う前記部分軌道の経由点において、隣接する部分軌道の速度又は加速度を一致させることを特徴とする軌道計画方法。
11. プロセッサとメモリを有する計算機で、多軸で構成されたロボットアームの手先の軌道を算出させるためのプログラムであって、
    ロボットアームの構成と、ロボットアームの関節を構成する軸の位置及び軸の姿勢を含むロボットアーム構成情報と、計画する軌道の開始姿勢における前記ロボットアームの各軸の角度を開始関節角度として設定された開始関節角度情報と、前記ロボットアームの手先の終点で、目標位置と、前記ロボットアームの手先の目標姿勢が設定された目標姿勢情報と、計画する軌道内で前記ロボットアームの手先が通過する位置と姿勢を含む経由点が設定された経由点姿勢情報と、を読み込んで、前記ロボットアームの手先の開始点から前
    記終点までの軌道を、前記経由点間を補間して生成する物理空間軌道計画ステップと、
    前記生成された軌道について、前記ロボットアーム構成情報を参照して前記ロボットアームの手先の姿勢及び位置から各軸の関節角度を算出し、前記軌道を関節角度空間に変換する逆運動学ステップと、
    前記関節角度空間に変換され前記軌道を平滑化する関節角度空間平滑化ステップと、
    を前記計算機に実行させるためのプログラム。

Images