JP2007222964A - モジュール制御用プログラム、制御システム - Google Patents

Abstract

【課題】 複数の構成要素からなる制御システムにおいて各構成要素間の位置及び姿勢と共に、各構成要素のワールド座標系における位置及び姿勢を特定する処理を簡単に行う。
【解決手段】 固有の座標系を用いた複数の構成要素を連結してなる制御システムの各構成要素に実装されるモジュール制御用プログラムであって、自己の構成要素座標系におけるワーキングポイントＷＰを受け取る情報インターフェース１４と、ワーキングポイントＷＰに所定のオフセットを与えるオフセット保持部１２と、上位の構成要素からのワールド座標系における位置・姿勢情報Ｍｎ−１を受け取るプログラム間インターフェース１１と、ワーキングポイントＷＰと所定のオフセットと位置・姿勢情報Ｍｎ−１とから、ワールド座標系における自己の位置・姿勢情報Ｍｎを演算する座標変換処理部１３と、位置・姿勢情報Ｍｎを下位の構成要素に渡すプログラム間インターフェース１５とを有する。
【選択図】 図６

Description

本発明は、それぞれ構成要素固有の座標系を有する複数の構成要素を統合して制御することによって動作する制御システム、当該各構成要素が動作するためのモジュール制御用プログラムに関する。

従来より、マニピュレータのような産業用ロボットのような複数のモジュールを統合して動作させる装置において、各モジュールが固有の座標系を有していることが多い。例えば、マニピュレータは、台座に固定された直交座標系と、マニピュレータに取り付けられた工具のような手先効果器のような根元を基準とする座標系とを有している。このように、マニピュレータ座標系と手先効果器座標系の相対的な位置・姿勢の関係は、マニピュレータ全体での動作によって変化するが、当該座標系間の相対的な関係は、逆運動学、順運動学に基づいて考察され、適切な変換行列を求めることによって適切に求めることができるとしている（例えば下記の特許文献１を参照）。

また、従来における非産業用ロボットを開発するに際しては、一般的に広い分野に使用されるといったように広い使用用途があり、ユーザターゲットが明確ではないので、ロボットの用途、使用場所に応じて各モジュールのハードウェア構成及び複数のモジュールからなるシステムのハードウェア構成を検討して設計することが行われていた。また、各モジュール及びシステムのソフトウェア設計においても、作製する毎に開発を行っていた。

しかしながら、このような開発手法は、作業効率が低いために、ロボット開発におけるハードウェア及びソフトウェアを複数のロボット要素に分割した上で、各ロボット要素の部品化（モジュール化）をして、できるだけ各モジュールを再利用する取り組みがなされている（下記の特許文献２及び非特許文献１を参照）。
特許第２７１８６７８号 特開２００５−１９６４２７号公報 ＮＥＤＯプロジェクト「ロボットの開発基盤となるソフトウェア上の基盤整備 成果報告書」（平成１４年度〜１６年度）

上述したようなロボットのうち、自律移動ロボットなどの空間・領域を自在に移動するロボットでは、時々刻々と変化する位置及び姿勢を特定するために空間に固定されたワールド座標系とロボットに固定されたロボット座標系とを設定し、当該ワールド座標系とロボット座標系との相対的な位置・姿勢を特定する必要がある。また、ロボットがアームやハンドといった複数の構成要素からなる場合には、当該構成要素ごとに、ワールド座標系とロボット座標系との相対的な位置・姿勢を特定する必要がある。

上述した従来の技術においては、隣接するロボット構成要素間の相対的な位置・姿勢を特定することはできるが、ロボット自体が移動した場合に、ワールド座標系におけるロボット構成要素の位置・姿勢が変化するのに対して、各ロボット構成要素が認識しないという状況が起こりうる。したがって、ロボット構成要素においても、ワールド座標系における自己の位置・姿勢を特定する必要がある。

そこで、本発明は、上述した実情に鑑みて提案されたものであり、複数の構成要素からなる制御システムにおいて各構成要素間の位置及び姿勢と共に、各構成要素のワールド座標系における位置及び姿勢を特定する処理を簡単に行うことができるモジュール制御用プログラム、制御システムを提供することを目的とする。

本発明は、それぞれが固有の座標系を用いた複数の構成要素を連結してなる制御システムにおいて各構成要素に実装されるモジュール制御用プログラムであって、上述の課題を解決するために、自己の構成要素座標系における基準位置情報を受け取る基準位置入力機能と、自己の構成要素座標系における基準位置に所定のオフセットを与えるオフセット保持機能と、複数の構成要素における上位のモジュール制御用プログラムから、当該上位のモジュール制御用プログラムが演算した、下位の構成要素の位置及び姿勢を演算するためのワールド座標系における位置及び姿勢を表す情報を受け取る情報入力機能と、基準位置入力機能で入力した自己の構成要素座標系における基準位置情報と、オフセット保持機能で保持している所定のオフセットと、情報入力機能で受け取った位置及び姿勢を表す情報とから、ワールド座標系における自己の構成要素の位置及び姿勢を演算する座標変換機能と、ワールド座標系における自己の構成要素の位置及び姿勢を表す情報を、複数の構成要素における下位のモジュール制御用プログラムに渡す情報出力機能とを構成要素に実装させる座標変換プログラムを有することを特徴とする。

また、本発明に係る他のモジュール制御用プログラムは、それぞれが固有の座標系を用いた複数の構成要素を連結してなる制御システムにおいて各構成要素に実装されるモジュール制御用プログラムであって、上述の課題を解決するために、自己の構成要素の動作を制御するためのアプリケーションプログラムと、アプリケーションプログラムからの命令に従って、ワールド座標系における自己の現在の位置及び姿勢を演算する座標変換プログラムとを有し、座標変換プログラムは、自己の構成要素座標系における基準位置情報を受け取る基準位置入力機能と、自己の構成要素座標系における基準位置に所定のオフセットを与えるオフセット保持機能と、複数の構成要素における上位のモジュール制御用プログラムから、当該上位のモジュール制御用プログラムが演算した、下位の構成要素の位置及び姿勢を演算するためのワールド座標系における位置及び姿勢を表す情報を受け取る情報入力機能と、基準位置入力機能で入力した自己の構成要素座標系における基準位置情報と、オフセット保持機能で保持している所定のオフセットと、情報入力機能で受け取った位置及び姿勢を表す情報とから、ワールド座標系における自己の構成要素の位置及び姿勢を演算する座標変換機能と、ワールド座標系における自己の構成要素の位置及び姿勢を表す情報を、複数の構成要素における下位のモジュール制御用プログラムに渡す情報出力機能とを構成要素に実装させることを特徴とする。

更に、本発明は、それぞれが固有の座標系を用いた複数の構成要素を統括制御する制御システムであって、上述の課題を解決するために、各構成要素は、自己の構成要素の動作を制御するためのアプリケーションプログラムと、アプリケーションプログラムからの命令に従って、ワールド座標系における自己の現在の位置及び姿勢を演算する座標変換プログラムとを有し、座標変換プログラムは、自己の構成要素座標系における基準位置情報を受け取る基準位置入力機能と、自己の構成要素座標系における基準位置に所定のオフセットを与えるオフセット保持機能と、複数の構成要素における上位のモジュール制御用プログラムから、当該上位のモジュール制御用プログラムが演算した、下位の構成要素の位置及び姿勢を演算するためのワールド座標系における位置及び姿勢を表す情報を受け取る情報入力機能と、基準位置入力機能で入力した自己の構成要素座標系における基準位置情報と、オフセット保持機能で保持している所定のオフセットと、情報入力機能で受け取った位置及び姿勢を表す情報とから、ワールド座標系における自己の構成要素の位置及び姿勢を演算する座標変換機能と、ワールド座標系における自己の構成要素の位置及び姿勢を表す情報を、複数の構成要素における下位のモジュール制御用プログラムに渡す情報出力機能とを有することを特徴とする。

本発明に係るモジュール制御用プログラム及び制御システムよれば、上位下位関係が設定された複数の構成要素における各構成要素に、ワールド座標系における位置及び姿勢を演算する座標変換プログラムを実装し、上位の座標変換プログラムから、当該上位のモジュール制御用プログラムが演算した下位の構成要素の位置及び姿勢を演算するためのワールド座標系における位置及び姿勢を表す情報を下位の座標変換プログラムに引き渡すことができる。したがって、複数の構成要素からなる制御システムにおいて各構成要素間の位置及び姿勢と共に、各構成要素のワールド座標系における位置及び姿勢を特定する処理を簡単に行うことができる。

以下、本発明の実施の形態について図面を参照して説明する。

本発明は、例えば図１に示すように、移動ロボットのロボット構成要素の動作及び幾何学的位置の変更に対して不変なワールド座標系で表現された家屋内などの所定空間１００内を移動する移動ロボットＡ，Ｂのロボット構成要素をモジュール化するためのモジュール制御用プログラム、及び、当該複数のロボット構成要素を統合して制御するためのロボット構成要素の統合システムに適用される。

移動ロボットＡ，Ｂのそれぞれは、図２に示すように、固定されたワールド座標系の所定空間１００に存在し、ロボット上部を回転させるターンテーブル１，当該ターンテーブル１上の３関節のリンク機構を有してマニピュレータで動作する左アーム２及び右アーム３，当該左アーム２及び右アーム３の先端に設けられた手先効果器としての左ハンド４，右ハンド５といった機能ごとのハードウェア的なロボット構成要素を有する。これらのロボット構成要素は、例えばアクチュエータ、センサ等で構成される。ロボット構成要素は、それぞれについてインストールされたモジュール制御用プログラムにおけるアプリケーションプログラムに従って回転駆動又は伸縮駆動といった動作を行う。また、動作は、モジュール制御用プログラムにおける座標変換プログラムによって、それぞれ固有の座標系によって管理される。

ロボット構成要素の座標系は、移動ロボットＡ，Ｂに固定されており、移動ロボットＡ，Ｂが移動しても不変なものである。このハードウェア構成要素の座標系は、移動ロボットＡ，Ｂの移動に伴ってワールド座標系から見た位置が変化され、移動ロボットＡ，Ｂの向きの変更によってワールド座標系から見た姿勢が変化する。

また、ロボット構成要素は、単一のモジュール統合プログラムに従って各ロボット構成要素間の位置関係、他の移動ロボットＡ，Ｂ間の位置関係等が管理される。このように、移動ロボットＡ，Ｂのソフトウェア的な構成要素は、複数のモジュール制御用プログラムからなる分散型オブジェクトとしたプログラム体系となっている。

ハードウェア的なロボット構成要素は、図３に示すように、移動ロボットＡ，Ｂにおける上下関係が定義されている。すなわち、所定空間１００に移動ロボットＡ，Ｂが存在して、当該移動ロボットＡ，Ｂごとに、ターンテーブル１，左アーム２及び右アーム３，左ハンド４及び右ハンド５の順で上下関係が定義されている。これらのハードウェア構成要素は、それぞれ協同して動作して、移動ロボットＡ，Ｂを動作させる。すなわち、ロボット構成要素ごとのモジュール制御用プログラムが動作して、当該ロボット構成要素ごとに動作すると共に、モジュール制御用プログラム間で演算結果を授受することによって上下関係において隣接するロボット構成要素のワールド座標系における位置・姿勢情報を得て、自己のモジュール制御用プログラムによってワールド座標系における位置・姿勢情報である演算結果を得る。

モジュール制御用プログラムは、ターンテーブル１，左アーム２，右アーム３，左ハンド４，右ハンド５といったハードウェア構成要素ごとに設計されて、図示しないメモリにインストールされる。また、このモジュール制御用プログラムとしては、所定空間１００のワールド座標系における座標変換プログラムがある。

モジュール制御用プログラムは、ハードウェア構成要素ごとのアプリケーションプログラムと、ハードウェア構成要素ごとの座標変換プログラムとを少なくとも含む。このアプリケーションプログラムは、例えば、モジュール制御用プログラムからの命令に従って各ハードウェア構成要素の駆動開始及び停止、ハードウェア構成要素の駆動力、駆動方向等を決定して、ハードウェア構成要素に制御信号を供給する処理等を行う。座標変換プログラムは、ハードウェア構成要素が動作したことによって変化するハードウェア構成要素の位置・姿勢を更新する処理を行う。この座標変換プログラムは、所定の座標系となっており、当該座標系における位置・姿勢を演算する。また、座標変換プログラムは、隣接して接続されたハードウェア構成要素の座標変換プログラムから、当該座標変換プログラムによって演算した位置・姿勢情報を入力し、当該位置・姿勢情報に基づいて、自己の位置・姿勢情報を更新して出力する。

モジュール統合プログラムは、図示しないメモリにインストールされ、移動ロボットＡ，Ｂ単位で必要な演算処理を行う。このモジュール統合プログラムは、モジュール統合プログラムのアプリケーションプログラムで演算された情報や、座標変換プログラムで演算された位置・姿勢情報を入力して、各モジュール制御用プログラムに命令を供給する。

このような移動ロボットＡ，Ｂは、図４に示すように、ハードウェア構成要素の座標変換プログラム１０Ａ，１０Ｂ，１０Ｃ，１０Ｄ，１０Ｅ，１０Ｆ，１０Ｇ，１０ＨごとにワーキングポイントＷＰが設定される。このワーキングポイントＷＰは、あるロボット構成要素に他のロボット構成要素を接続する基準となる点であり、各ロボット構成要素固有の座標系で表現される位置及び姿勢を要素として有している。なお、以下の説明では、移動ロボットＡ，Ｂ上に設定されたワーキングポイントＷＰは、移動ロボットＡ，Ｂに対して固定されており、移動ロボットＡ，Ｂの動作に対して不変であるものとする。

ターンテーブル１の座標系は、移動ロボットＡ，Ｂ上に設定されたワーキングポイントＷＰから所定のオフセットだけ離れた点を原点としている。このオフセットは、ゼロであることが望ましいが、幾何学的制約あるいは直感的に理解しやすい位置にロボット構成要素固有の座標系原点を設定することが望ましいという理由で設けられている。また、このターンテーブル１の座標系の原点は、ターンテーブル１の動作に対して不変となる。また、複数の座標系を定義するに際して、移動ロボットＡ，Ｂとターンテーブル１のようにワーキングポイントＷＰ及びオフセットを媒介とした幾何学的位置の関係と共に、移動ロボットＡ，Ｂを親のロボット構成要素に設定し、ターンテーブル１を子のロボット構成要素に設定する。

また、左アーム２，右アーム３，左ハンド４，右ハンド５の座標系についても、ターンテーブル１の座標系の設定方法と同様に、ワーキングポイントＷＰからオフセットを介したロボット構成要素固有の座標系原点と、他のロボット構成要素との幾何学的位置の関係と、親子関係を設定することになる。

このように、各ロボット構成要素に固有の座標系原点、幾何学的位置関係及び親子関係を設定することによって、左アーム２及び右アーム３は、ワールド座標系においてターンテーブル１より上方向に座標系の原点があり、ターンテーブル１が動作するとターンテーブル座標系で表現されたアーム固有の座標系の位置及び姿勢が変化することになる。また、左ハンド４及び右ハンド５は、ワールド座標系において左アーム２及び右アーム３より上方向に座標系の原点があり、左アーム２及び右アーム３が動作するとアーム座標系で表現されたハンド固有の座標系の位置及び姿勢が変化することになる。

なお、各ロボット構成要素におけるワーキングポイントＷＰは、当該ワーキングポイントＷＰが設定されるロボット構成要素上に設定するが、ロボット構成要素の座標系で表現され得る点であれば、任意の空間上に座標系の原点を設定しても良い。

このようにロボット構成要素について設定された固有の座標系原点、幾何学的位置関係及び親子関係は、モジュール制御用プログラムの座標変換プログラムと同じオブジェクトに属するものとして、各ロボット構成要素についてのメモリに記憶される。各ロボット構成要素は、自己のワーキングポイントＷＰにどのようなロボット構成要素が連結されているかという情報を得ることなく動作する。

各ロボット構成要素の座標変換プログラムは、モジュール制御用プログラムからワーキングポイントＷＰが与えられ、図５に示すように、隣接する上位（親）の座標変換プログラムから送られた下位の構成要素の位置及び姿勢を演算するためのワールド座標系における位置・姿勢情報Ｍ＿ｎを入力し、当該位置・姿勢情報Ｍ＿ｎ、ワーキングポイントＷＰ＿ｎ及び予め設定されたオフセットＯ＿ｎを用いて、自己の位置・姿勢情報Ｍ＿ｎを演算して、下位（子）の座標変換プログラムに送る。

ここで、あるハードウェア構成要素としてのロボット構成要素をＲＴ＿ｎとし、当該ロボット構成要素ＲＴ＿ｎの親のロボット構成要素をＲＴ＿ｎ−１、子のロボット構成要素をＲＴ＿ｎ_＋１とする。また、ｎを０とした仮想的なロボット構成要素をワールド座標系とする。図５に示す例では、ｎ＝１が移動ロボットＡであり、ｎ＝２がターンテーブル１であり、ｎ＝３が左アーム２であり、ｎ＝４が左ハンド４であり、ｎ＝５が右アーム３であり、ｎ＝６が右ハンド５であり、ｎ＝Ｎが移動ロボットＢである場合を示している。また、モジュール制御用プログラムをＲＴＣ_ｎと表記し、ワーキングポイントＷＰをＷＰ＿ｎと表記し、座標変換プログラムをＴｒａｎｓ＿ｎと表記する。

この座標変換プログラムＴｒａｎｓ＿ｎは、図６に示す座標変換プログラム１０であり、座標変換プログラム１０は、ロボット構成要素ＲＴ＿ｎの動作を制御するためのアプリケーションプログラムからの命令に従って、ワールド座標系における自己の現在の位置及び姿勢を演算する座標変換プログラムである。この座標変換プログラム１０は、自己の座標系におけるワーキングポイントＷＰ＿ｎと共に、ワールド座標系における自己の現在の位置及び姿勢を演算するコマンドを受け取る基準位置入力機能である情報インターフェース１４と、自己の座標系におけるワーキングポイントＷＰ＿ｎに所定のオフセットを与えるオフセット保持機能であるオフセット保持部１２と、複数のロボット構成要素ＲＴ＿ｎにおける上位の座標変換プログラム１０から、当該上位の座標変換プログラム１０が演算した、下位のロボット構成要素ＲＴ＿ｎの位置及び姿勢を演算するためのワールド座標系における位置及び姿勢情報Ｍｎ−１を受け取る情報入力機能であるプログラム間インターフェース１１と、情報インターフェース１４で入力した自己の座標系におけるワーキングポイントＷＰ＿ｎと、オフセット保持部１２で保持している所定のオフセットと、プログラム間インターフェース１１で受け取った位置・姿勢情報Ｍ＿ｎ−１とから、ワールド座標系における自己の位置及び姿勢である位置・姿勢情報Ｍ＿ｎを演算する座標変換機能である座標変換処理部１３と、ワールド座標系における位置・姿勢情報Ｍ＿ｎを、下位の座標変換プログラムに渡す情報出力機能であるプログラム間インターフェース１５とを有する。なお、基準位置入力機能は、ワールド座標系における自己の現在の位置及び姿勢を演算することを命令するコマンドを入力しなくても、ワーキングポイントＷＰ＿ｎが与えられ、子の座標変換プログラムからの位置・姿勢情報Ｍ＿ｎを受信したことに応じて自動的に演算を開始しても良い。

この座標変換プログラムＴｒａｎｓ＿ｎは、情報インターフェース１４によって、自己のモジュール制御用プログラムＲＴＣ＿ｎからワーキングポイントＷＰ＿ｎを得る。このワーキングポイントＷＰ＿ｎは、少なくとも、ロボット構成要素ＲＴ＿ｎの固有の座標系で表現される位置・姿勢情報を含む。

また、座標変換プログラムＴｒａｎｓ＿ｎは、親側の座標変換プログラムＴｒａｎｓ＿ｎ−１からプログラム間インターフェース１１によって、位置・姿勢情報Ｍ＿ｎ−１とワーキングポイントＷＰ＿ｎとを取得する。この位置・姿勢情報Ｍ＿ｎ−１は、ロボット構成要素ＲＴ＿ｎの固有の座標系で表現されたワーキングポイントＷＰ＿ｎをワールド座標系で表現するための４×４変換行列であり、下記の式１のように、
Ｍ＿ｎ＝Ｍ＿ｎ−１・Ｏ＿ｎ・［ＷＰ＿ｎ］ （式１）
で表現される。この式１において、［ＷＰ＿ｎ］は下記の数１で示すような４×４行列であって、ロボット構成要素ＲＴ＿ｎのワールド座標系における位置及び姿勢を含んでいる。

例えば、ターンテーブル１が回転して、右アーム３の座標系がターンテーブル１の座標系に対して、ターンテーブル１の回転軸に対してαだけ回転した場合、ターンテーブル１の回転角度が位置・姿勢情報Ｍ＿ｎ−１で与えられて、右アーム３の座標変換プログラムに供給され、右アーム３の座標変換プログラムは、当該位置・姿勢情報Ｍ＿ｎ−１に対してオフセットＯ＿ｎだけ平行移動させた行列式を演算し、次に、ワーキングポイントＷＰ＿ｎだけ平行移動させた４×４の行列式を演算する。これによって、ターンテーブル１がαだけ回転し、オフセットＯ＿ｎ及びワーキングポイントＷＰ＿ｎだけ移動させたワールド座標系における自己の位置・姿勢情報Ｍ＿ｎを作成できる。

ここで、上位のロボット構成要素の回転動作によって自己の位置・姿勢情報Ｍ＿ｎが変化した時の行列式は、下記の式２で表現され、上位のロボット構成要素の平行移動動作によって自己の位置・姿勢情報Ｍ＿ｎが変化した時の行列式は、下記の式３で表現される。

したがって、座標変換プログラムＴｒａｎｓ＿ｎは、位置・姿勢情報Ｍ＿ｎによって式２と式３とが混合された４×４行列式を得た場合に、式３で表現される行列式を乗算し、且つワーキングポイントＷＰ＿ｎを座標系原点としたワールド座標系の位置・姿勢情報Ｍ＿ｎを演算できる。また、座標変換プログラムＴｒａｎｓ＿ｎは、位置・姿勢情報Ｍ＿ｎ−１にオフセットＯ＿ｎを与えた演算結果を得て、当該演算結果を用いてワーキングポイントＷＰ＿ｎをワールド座標系で表現した位置・姿勢情報ＡＮＳ＿ｎを演算する。この位置・姿勢情報ＡＮＳ＿ｎは、下記の式４に示すように、
ＡＮＳ＿ｎ＝Ｍ＿ｎ−１・Ｏ＿ｎ・ＷＰ＿ｎ （式４）
で表現される。ここで、式４におけるＷＰ＿ｎは、ロボット構成要素固有の座標系原点からの位置座標を示す（ｘ，ｙ，ｚ）と、ロボット構成要素固有の座標系原点に対する姿勢を示す（α，β，γ）とで表現される。この位置・姿勢情報ＡＮＳ_ｎは、図６における情報インターフェース１６によって、モジュール統合プログラムに出力される。なお、下位の座標変換プログラムＴｒａｎｓ＿ｎに渡す位置・姿勢情報Ｍ＿ｎは、４×４変換行列で表現された情報であり、モジュール統合プログラム等に渡す位置・姿勢情報ＡＮＳ＿ｎは、変換行列ではなく自己のワールド座標系で表現した情報である。

ワーキングポイントＷＰ＿ｎは、所定時間毎にロボット構成要素ＲＴ＿ｎのモジュール制御用プログラムから座標変換プログラムＴｒａｎｓ＿ｎに供給され、座標変換処理部１３は、所定時間ごとに自己のワールド座標系における位置・姿勢情報Ｍ＿ｎ及び位置・姿勢情報ＡＮＳ_ｎを演算する。この場合、ワーキングポイントＷＰ＿ｎの値が実際には変更されていなくても、ワーキングポイントＷＰ＿ｎの送信が行われるので計算機（ＣＰＵ）のリソースや通信トラフィックを無駄に使用する弊害があるものの、当該ワーキングポイントＷＰ＿ｎの送信動作を通信エラーなどの検知用としてＣＰＵのリソース及び通信トラフィックを割り当てることもできる。

一方、例えば、ワーキングポイントＷＰ＿ｎの値が変更された場合のみ、当該ワーキングポイントＷＰ＿ｎをロボット構成要素ＲＴ＿ｎから座標変換プログラムＴｒａｎｓ＿ｎに供給するようにしても良い。

また、移動ロボットＡ，Ｂの異常動作や通信エラーの検出手段を他に持つ場合や、移動ロボットＡ，Ｂの異常動作が即座に致命的な事故につながらないような移動ロボットＡ，Ｂではこの方式を採用しても良い。更に、ロボット構成要素ＲＴ＿ｎ間又は移動ロボットＡ，Ｂ間のデータの送受信は、プッシュ型又はプル型のいずれであっても良い。例えば、プル型を採用して、座標変換プログラムＴｒａｎｓ＿ｎは、下位のロボット構成要素ＲＴ＿ｎの座標変換プログラムＴｒａｎｓ＿ｎ−１からの位置・姿勢情報Ｍ＿ｎの更新要求を受けた時に、ワーキングポイントＷＰ＿ｎを受け取って、位置・姿勢情報Ｍ＿ｎを演算することで、ロボット構成要素ＲＴ＿ｎ間のデータトラフィックを少なくして、自己の位置・姿勢情報Ｍ＿ｎの更新を行うことができる。

次に、各ロボット構成要素ＲＴ＿ｎの座標変換プログラムＴｒａｎｓ＿ｎについて説明する。

座標変換プログラムＴｒａｎｓ＿ｎは、ロボット構成要素ＲＴ＿ｎの固有の座標系の原点を設定するワーキングポイントＷＰ＿ｎを変更するオフセットＯ＿ｎの情報を予め記憶している。このオフセットＯ＿ｎは、座標変換プログラムＴｒａｎｓ＿ｎを実装するに際して、内部パラメータとして設計者によって与えられる。すなわち、オフセットＯ＿ｎは、設計者がロボット構成要素ＲＴ＿ｎと座標変換プログラムＴｒａｎｓ＿ｎとを関連付けて実装する際のモジュール制御用プログラムの初期値として設定される。

座標変換プログラムＴｒａｎｓ＿ｎは、オフセットＯ＿ｎのデータを参照して、自己のワーキングポイントＷＰ＿ｎを変更し、当該ワーキングポイントＷＰ＿ｎと親のロボット構成要素ＲＴ＿ｎ−１からの位置・姿勢情報Ｍ＿ｎ−１とから、ロボット構成要素ＲＴ＿ｎの固有座標系における位置及び姿勢を表す４×４変換行列を生成する。このとき、座標変換プログラムＴｒａｎｓ＿ｎは、オフセットＯ＿ｎによって位置・姿勢情報Ｍ＿ｎ−１を変換した変換行列を作成し、モジュール制御用プログラムＲＴＣ＿ｎから与えられたワーキングポイントＷＰ＿ｎを用いて自己の位置及び姿勢を示す位置・姿勢情報Ｍ＿ｎを作成する。また、座標変換プログラムＴｒａｎｓ＿ｎは、自己の位置・姿勢情報Ｍ＿ｎから、位置及び姿勢を示す要素情報を含む位置・姿勢情報ＡＮＳ＿ｎを作成して、モジュール統合プログラム等に出力する。

ここで、図４に示すように、移動ロボットＡ，Ｂにおいて最上位の座標変換プログラムＴｒａｎｓ＿０（ｎ＝０、座標変換プログラム１０Ａ）については、子のロボット構成要素ＲＴ＿ｎが存在しないことから、子の座標変換プログラムＴｒａｎｓ＿ｎが存在しなく、位置・姿勢情報Ｍ＿ｎが供給されない。なお、ｎ＝０の場合とは、ロボット構成要素ＲＴ＿ｎがワールド座標系そのものの仮想的なロボット構成要素ＲＴ＿ｎに該当する。この場合、座標変換プログラムＴｒａｎｓ＿０には、位置・姿勢情報Ｍ＿ｎとして４×４の単位行列を与えて、当該座標変換プログラムＴｒａｎｓ＿０によって位置・姿勢情報Ｍ＿０及び位置・姿勢情報ＡＮＳ＿０を作成させる。

このように、本発明を適用したモジュール制御用プログラムは、ロボット構成要素ＲＴ＿ｎごとに座標変換プログラムＴｒａｎｓ＿ｎを設けて、移動ロボットＡ，Ｂのロボット構成要素ＲＴ＿ｎ間を座標変換プログラムＴｒａｎｓ＿ｎを介して接続することができる。これによって、実際のロボット構成要素ＲＴ＿ｎの連結順序に拘わらず、座標変換プログラムＴｒａｎｓ＿ｎ間でロボット構成要素ＲＴ＿ｎのワールド座標系における位置及び姿勢を授受するような共通のモジュール制御用プログラムをロボット構成要素ＲＴ＿ｎに実装するだけで、ロボット構成要素ＲＴ＿ｎ間でワールド座標系における位置及び姿勢を認識させることができる。

すなわち、上述した一例では、最上位のロボット構成要素ＲＴ＿ｎから、移動ロボットＡ，Ｂ（ワールド座標系）、ターンテーブル１、左アーム２及び右アーム３、左ハンド４及び右ハンド５という親子関係の順序であったが、移動ロボットＡ，Ｂ、右アーム３及び左ハンド４、ターンテーブル１、左ハンド４及び右ハンド５といった親子関係の順序に変更しても、各ロボット構成要素ＲＴ＿ｎから座標変換プログラムＴｒａｎｓ＿ｎに適切なオフセットＯ＿ｎ及びワーキングポイントＷＰ＿ｎを与えるだけで、モジュール制御用プログラムに大きな変更を与えるといった作業をなくすことができる。

更に、新たなロボット構成要素ＲＴ＿ｎとしてターンテーブル１上にもう一組のアーム及びハンドを追加する場合であっても、当該新たなアームに、ワールド座標系における移動ロボットＡ，Ｂの位置及び姿勢を演算し、当該ワールド座標系における位置及び姿勢からターンテーブル１の固有座標の位置及び姿勢を演算した上で自己のアームの固有の座標系における位置及び姿勢を演算するモジュール制御用プログラムを実装する必要なく、さらには、新たなハンドに、ワールド座標系における移動ロボットＡ，Ｂの位置及び姿勢を演算し、当該ワールド座標系における位置及び姿勢からターンテーブル１の固有座標の位置及び姿勢を演算し、アームの固有の座標系における位置及び姿勢を演算した上で自己の固有の座標系における位置及び姿勢を演算するモジュール制御用プログラムを実装する必要をなくすことができる。

なお、上述の実施の形態は本発明の一例である。このため、本発明は、上述の実施形態に限定されることはなく、この実施の形態以外であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計等に応じて種々の変更が可能であることは勿論である。

本発明を適用したモジュール制御用プログラムを実装した移動ロボットのワールド座標系について説明するための図である。 本発明を適用したモジュール制御用プログラムを実装した移動ロボットの上下関係を説明する図である。 本発明を適用したモジュール制御用プログラムを実装した移動ロボットの上下関係を説明する他の図である。 本発明を適用したモジュール制御用プログラムにワーキングポイントを与えて、隣接する座標変換プログラム間でワールド座標系における位置・姿勢情報を更新する様子を説明するためのブロック図である。 本発明を適用したモジュール制御用プログラムにワーキングポイントを与えて、隣接する座標変換プログラム間でワールド座標系における位置・姿勢情報を更新する様子を説明するための他のブロック図である。 本発明を適用したモジュール制御用プログラムにおける座標変換プログラムの機能を説明するためのブロック図である。

符号の説明

１ ターンテーブル
２ 左アーム
２ 左アーム
３ 右アーム
４ 左ハンド
５ 右ハンド
１０ 座標変換プログラム
１１ プログラム間インターフェース
１２ オフセット保持部
１３ 座標変換処理部
１４ 情報インターフェース
１５ プログラム間インターフェース
１６ 情報インターフェース
１００ 所定空間

Claims (6)

1. それぞれが固有の座標系を用いた複数の構成要素を連結してなる制御システムにおいて各構成要素に実装されるモジュール制御用プログラムであって、
   自己の構成要素座標系における基準位置情報を受け取る基準位置入力機能と、
   前記自己の構成要素座標系における基準位置に所定のオフセットを与えるオフセット保持機能と、
   前記複数の構成要素における上位のモジュール制御用プログラムから、当該上位のモジュール制御用プログラムが演算した、下位の構成要素の位置及び姿勢を演算するためのワールド座標系における位置及び姿勢を表す情報を受け取る情報入力機能と、
   前記基準位置入力機能で入力した自己の構成要素座標系における基準位置情報と、前記オフセット保持機能で保持している所定のオフセットと、前記情報入力機能で受け取った位置及び姿勢を表す情報とから、ワールド座標系における自己の構成要素の位置及び姿勢を演算する座標変換機能と、
   ワールド座標系における自己の構成要素の位置及び姿勢を表す情報を、複数の構成要素における下位のモジュール制御用プログラムに渡す情報出力機能と
   を構成要素に実装させる座標変換プログラムを有すること
   を特徴とするモジュール制御用プログラム。
2. それぞれが固有の座標系を用いた複数の構成要素を連結してなる制御システムにおいて各構成要素に実装されるモジュール制御用プログラムであって、
   自己の構成要素の動作を制御するためのアプリケーションプログラムと、
   前記アプリケーションプログラムからの命令に従って、ワールド座標系における自己の現在の位置及び姿勢を演算する座標変換プログラムとを有し、
   前記座標変換プログラムは、
   自己の構成要素座標系における基準位置情報を受け取る基準位置入力機能と、
   前記自己の構成要素座標系における基準位置に所定のオフセットを与えるオフセット保持機能と、
   前記複数の構成要素における上位のモジュール制御用プログラムから、当該上位のモジュール制御用プログラムが演算した、下位の構成要素の位置及び姿勢を演算するためのワールド座標系における位置及び姿勢を表す情報を受け取る情報入力機能と、
   前記基準位置入力機能で入力した自己の構成要素座標系における基準位置情報と、前記オフセット保持機能で保持している所定のオフセットと、前記情報入力機能で受け取った位置及び姿勢を表す情報とから、ワールド座標系における自己の構成要素の位置及び姿勢を演算する座標変換機能と、
   ワールド座標系における自己の構成要素の位置及び姿勢を表す情報を、複数の構成要素における下位のモジュール制御用プログラムに渡す情報出力機能と
   を構成要素に実装させるモジュール制御用プログラム。
3. それぞれが固有の座標系を用いた複数の構成要素を統括制御する制御システムであって、
   各構成要素は、
   自己の構成要素の動作を制御するためのアプリケーションプログラムと、
   前記アプリケーションプログラムからの命令に従って、ワールド座標系における自己の現在の位置及び姿勢を演算する座標変換プログラムとを有し、
   前記座標変換プログラムは、
   自己の構成要素座標系における基準位置情報を受け取る基準位置入力機能と、
   前記自己の構成要素座標系における基準位置に所定のオフセットを与えるオフセット保持機能と、
   前記複数の構成要素における上位のモジュール制御用プログラムから、当該上位のモジュール制御用プログラムが演算した、下位の構成要素の位置及び姿勢を演算するためのワールド座標系における位置及び姿勢を表す情報を受け取る情報入力機能と、
   前記基準位置入力機能で入力した自己の構成要素座標系における基準位置情報と、前記オフセット保持機能で保持している所定のオフセットと、前記情報入力機能で受け取った位置及び姿勢を表す情報とから、ワールド座標系における自己の構成要素の位置及び姿勢を演算する座標変換機能と、
   ワールド座標系における自己の構成要素の位置及び姿勢を表す情報を、複数の構成要素における下位のモジュール制御用プログラムに渡す情報出力機能とを有すること
   を特徴とする制御システム。
4. 前記基準位置入力機能は、所定時間ごとに基準位置情報を入力し、
   前記座標変換機能は、当該基準位置情報を用いて自己の構成要素の位置及び姿勢を演算すること
   を特徴とする請求項１又は請求項２に記載のモジュール制御用プログラム。
5. 前記基準位置入力機能は、自己の構成要素の基準位置が変更した場合に当該基準位置情報を入力し、
   前記座標変換機能は、当該基準位置情報を用いて自己の構成要素の位置及び姿勢を演算すること
   を特徴とする請求項１又は請求項２に記載のモジュール制御用プログラム。
6. 前記座標変換機能は、下位の構成要素からの要求を受けた場合に、自己の構成要素の位置及び座標を演算し、
   前記情報出力機能は、当該下位のモジュール制御用プログラムに自己の位置及び姿勢を表す情報を渡すこと
   を特徴とする請求項１又は請求項２に記載のモジュール制御用プログラム。
   

Images