(57)【要約】【課題】ロボットを構成する２つの制御ユニットＡ，Ｂにおいて、制御ユニットＢが制御ユニットＡ上に設置されている場合に、両方の制御ユニットを同時に動かしながら、制御ユニットＢの制御点に対して所望の補間動作ができるようにする。【解決手段】制御ユニットＢのユニットコントローラＢに対して、制御ユニットＡの制御点ＡＣに対する指令Ａも与える。ユニットコントローラＢは、指令Ａを制御周期ごとに補間計算し、制御ユニットＢのユニット座標系Ｂにおける制御点ＢＣの補間位置を計算することで、制御ユニットＢのアクチュエータの補間位置を計算し、アクチュエータを制御するという手順で処理する。【選択図】図１
2012045636.tif 000002
【特許請求の範囲】
【請求項１】
２つのアクチュエータを制御する制御ユニットＡおよび制御ユニットＢと、前記制御ユニットＡおよび前記制御ユニットＢを統括制御する統括コントローラとが互いにネットワークで接続され、前記制御ユニットＡおよび前記制御ユニットＢには各々ローカル座標系であるユニット座標系Ａおよびユニット座標系Ｂを持ち、前記制御ユニットＡの制御点ＡＣの移動に伴い前記ユニット座標系Ｂの原点ＢＯが移動するように構成されており、前記制御ユニットＡは制御点ＡＣに対する補間指令を前記統括コントローラから入力し前記制御ユニットＡに属するアクチュエータを移動制御し、前記制御ユニットＢは前記ユニット座標系Ｂにおける前記制御ユニットＢの制御点ＢＣに対する補間指令を前記統括コントローラから入力し前記制御ユニットＢに属するアクチュエータを前記ユニット座標系Ａにおける補間軌道に沿って前記制御点ＢＣを移動制御する補間方法において、
前記制御ユニットＢは、
前記ユニット座標系Ａにおける前記制御点ＢＣの補間点ＢＣＴを計算し、
前記ユニット座標系Ａにおける前記原点ＢＯの補間点ＢＯＴを計算し、
前記補間点ＢＯＴを基準にして前記ユニット座標系Ｂにおける前記補間点ＢＣＴの補間点ＢＯＣＴを計算し、
前記補間点ＢＯＣＴに基づいて前記前記制御ユニットＢに属するアクチュエータの補間位置を計算し、
前記補間位置に前記アクチュエータを制御する、
という手順で処理することを特徴とする前記制御ユニットＢの前記制御点ＢＣの補間方法。
【請求項２】
前記補間点ＢＯＴは、前記統括コントローラから入力した前記ユニット座標系Ａにおける前記原点ＢＯの現在位置と目標位置と該目標位置までの補間形状と指令速度とから計算し、
前記補間点ＢＣＴは、前記統括コントローラから入力した前記ユニット座標系Ａにおける前記制御点ＢＣの目標位置と目標位置までの補間形状と指令速度とから計算する、
ことを特徴とする請求項１記載の補間方法。
【請求項３】
２つのアクチュエータを制御する制御ユニットＡおよび制御ユニットＢと、前記制御ユニットＡおよび前記制御ユニットＢを統括制御する統括コントローラとが互いにネットワークで接続され、前記制御ユニットＡおよび前記制御ユニットＢには各々ローカル座標系であるユニット座標系Ａおよびユニット座標系Ｂを持ち、前記制御ユニットＡの制御点ＡＣの移動に伴い前記ユニット座標系Ｂの原点ＢＯが移動するように構成されており、前記制御ユニットＡは制御点ＡＣに対する補間指令を前記統括コントローラから入力し前記制御ユニットＡに属するアクチュエータを移動制御し、前記制御ユニットＢは前記ユニット座標系Ｂにおける前記制御ユニットＢの制御点ＢＣに対する補間指令を前記統括コントローラから入力し前記制御ユニットＢに属するアクチュエータを前記ユニット座標系Ａにおける補間軌道に沿って前記制御点ＢＣを移動制御するロボットにおいて、
前記制御ユニットＢは、
前記ユニット座標系Ａにおける前記制御点ＢＣの目標位置と目標位置までの補間形状と指令速度と前記ユニット座標系Ａにおける前記ユニット座標系Ｂの原点ＢＯの現在位置と目標位置と目標位置までの補間形状と指令速度とを前記統括コントローラから入力する補間指令入力部と、
前記ユニット座標系Ａにおける前記制御点ＢＣの補間位置を計算するユニット座標系Ａにおける制御点補間処理部と、
前記ユニット座標系Ａにおける前記ユニット座標系Ｂの原点ＢＯの補間位置を計算するユニット座標系Ａにおける原点補間処理部と、
前記ユニット座標系Ｂにおける前記制御点ＢＣの補間位置を計算する、ユニット座標系Ｂにおける制御点補間処理部と、
前記制御ユニットＢに属するアクチュエータの補間位置を計算するアクチュエータ補間処理部と、
前記制御ユニットＢに属するアクチュエータを制御するアクチュエータ制御部と、を備え、
前記制御ユニットＢに属するアクチュエータの前記制御点ＢＣを補間制御することを特徴とするロボット。
【請求項４】
前記統括コントローラは、
前記制御ユニットＡに対して前記ユニット座標系Ａにおける前記制御点ＡＣの目標位置と目標位置までの補間形状と指令速度を、と与え、
前記制御ユニットＢに対して前記ユニット座標系Ａにおける前記制御ユニットＡの制御点ＡＣと目標位置と目標位置までの補間形状と指令速度と前記制御ユニットＢの制御点ＢＣの目標位置と目標位置までの補間形状と指令速度とを与える指令生成部を備え、
ることを特徴とする請求項３記載のロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットを構成する制御ユニット間の協調制御に関する。
【背景技術】
【０００２】
これまで、産業用途においては、主に溶接や搬送といった単一の作業を処理するアームタイプの単機能ロボット（以降、「アームロボット」と言う）が多く用いられてきた。この場合、アームロボットの位置や姿勢を直接制御する装置、いわゆるロボットコントローラが制御すべきサーボモータの数は、多くとも６、７程度であった。
しかしながら今日では、ロボットに求められる作業が複雑化かつ多機能化している。例えば、複数マニピュレータを協調動作させた部品組み立て作業といったものがそうである。さらに、マニピュレータ先端に装着されるエンドエフェクタについても、対象を掴むあるいは放すといった単純な作業以外に、ペットボトルのキャップのような対象を掴みながら回転させるといった作業が要求されるようになってきており、サーボモータが１個装着されたグリッパタイプだけでなく、サーボモータが多数（例えば、９）装着されたハンドタイプのエンドエフェクタも制御する必要がでてきた。
このように、ロボットに求められる作業が複雑化かつ多機能化しているため、ロボットに装着されるサーボモータの数が増大している。例えば、全方向に移動可能な移動機構を備え、ハンドタイプのエンドエフェクタを取り付けた図７に示すような移動型双腕ロボットの場合、使用されるサーボモータの数は
移動機構              ３
腕           １４（右、左ごとに７）
エンドエフェクタ    １８（右、左ごとに９）
--------------------------------------------------
合計         ３５
となり、コントローラで使用するマイコンの性能上の限界から、1つのコントローラで処理することが困難になってきている。
この一般的な技術課題を解決するために、ロボットを「ユニット」と呼ばれる複数の機能ブロックに分割し、それぞれのユニットには、ユニットを制御するユニットコントローラを内蔵させ、ロボットの処理を、機能単位でこれらユニットコントローラに分散させることが行われている。例えば、前述した双腕アームロボットの場合、移動機能を移動ユニットに、アーム機能はアームユニットに、エンドエフェクタ機能をエンドエフェクタユニットにそれぞれ分割し、すべてのユニットの動作を管理統括する統括コントローラを設けることで、それぞれのユニットコントローラで制御すべきサーボモータの数を軽減し、統括コントローラにより双腕アームロボット全体の制御が可能としている。
【０００３】
このロボット全体の機能構成を図３に基づいて説明する。図３は移動ユニット３０１上にアームユニット３０２が、さらにアームユニット３０２上にエンドエフェクタユニット３０３が設置された移動型アームユニット３００の模式図である。
そして、移動ユニット３０１、アームユニット３０２、エンドエフェクタユニット３０３は、それぞれ自ユニットを制御するために必要な機能のみを内蔵している。
例えば、サーボモータで制御される３つの車輪を持つ移動ユニット３０１は、床面のある一点を原点とする三次元直交座標系（以降、「移動ユニット座標系」と言う）３１１に対して、移動ユニット制御点３２２の位置・姿勢（X，Y，θ）を補間制御するためのインタフェース（直線補間指令や円弧補間指令など）を持ち、これらの指令データを3つの車輪の角度データに変換する逆運動学変換処理を内蔵している。これにより移動ユニット３０１は、統括コントローラから、位置・姿勢に関する補間指令を受信すると、現在位置・姿勢に対して、目標位置・姿勢を計算し、逆運動学変換処理により制御周期ごとの車輪の角度を計算し、サーボモータを制御することができる。
アームユニット３０２についても同様に、アームユニット内部の一点を原点とする三次元直交座標系（以降、「アームユニット座標系」と言う）３１２に対して、アームユニット制御点３２３の位置・姿勢（X，Y，Z, roll, pitch, yaw）を補間制御するためのインタフェース（直線補間指令や円弧補間指令など）を持ち、これらの指令データを各関節の角度データに変換する逆運動学変換処理を内蔵している。これによりアームユニット３０２は、統括コントローラから、位置・姿勢に関する補間指令を受信すると、現在位置・姿勢に対して、目標位置・姿勢を計算し、逆運動学変換処理により制御周期ごとの各関節の角度を計算し、サーボモータを制御することができる。
エンドエフェクタユニット３０３についても同様であるため、説明は省略する。
【０００４】
このようなユニットの組み合わせにより構成されるロボットのアームユニット制御点の位置・姿勢を補間制御するために、一般的にはいくつかの方法が考えられる。例えば、図４に示すように、移動ユニット３０１を移動ユニット座標系３１１上で円弧に沿って移動させながらアームユニット制御点３２３を直線的に移動させたい場合、従来は以下のような方法がとられていた。
（１）移動ユニット３０１、アームユニット３０２の機能分担に応じて、移動ユニット３０１のコントローラには移動ユニット座標系３１１上の位置・姿勢指令、アームユニット３０２のコントローラにはアームユニット座標系３１２上の位置・姿勢指令をそれぞれ与える方法。
（２）  アームユニット３０２のコントローラに、移動ユニット座標系３１１上の制御点位置・姿勢指令のためのインタフェースと、移動ユニット３０１の位置・姿勢情報を入力する通信部を設ける。アームユニット３０２のコントローラは、統括コントローラから与えられた移動ユニット座標系３１１上のアームユニット制御点３２３の位置・姿勢指令と、移動ユニット３０１のコントローラから受信する制御周期ごとの位置データを元に、アームユニット制御点３２３のアームユニット座標系３１２上の位置・姿勢を計算し、制御周期ごとに全関節の角度を計算しサーボモータを制御する方法。
特に、（２）の方法を実施するためには、一般的には、特許文献１に示すように、2つの協調するロボットの制御部間で制御周期ごとに位置データを送受信する必要がある。この従来技術の適用例の模式図を図６に示す。図６において、２つのアームユニット６０１、６０２を協調させる場合、アームユニット６０１をマスタアーム、アームユニット６０２をスレーブアームとし、マスタアームのユニット制御点６２１の位置データを、一定周期でマスタアームのアームユニットコントローラ６１１からスレーブアームのアームユニット６０２のアームユニットコントローラ６１２へ送信することで、スレーブアームの制御周期ごとの位置データを計算し、アームユニット６０２のアクチュエータを制御する。この方法を図３の移動型アームロボット３００に適用するならば、移動ユニット３０１の制御点データをアームユニット３０２へ一定周期で送信することになる。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特許第３０８６０８２号公報（第７頁、図１）
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、これらの方法には以下に述べるような課題がある。
まず、（１）の方法では、アームユニットのコントローラに与えるべきアームユニット座標系上の位置・姿勢指令を作成するのが難しく、かつ軌跡精度が悪いという課題がある。一般的に、統括コントローラからアームユニットコントローラへ与える指令は、アームユニット座標系における目標位置・姿勢、および指令速度である。図４に示す移動ユニット３０１を円弧にそって移動させながらアームユニット制御点３２３を移動ユニット座標系３１１に対して直線的に移動させる場合、移動ユニット３０１の動作に従ってアームユニット座標系原点３２２も移動するため、アームユニット座標系３１２から見たアームユニット制御点３２３の位置・姿勢の動作は、一般的には単純に直線で表されるような軌跡ではなく、例えば図５に示すような複雑な軌跡を描く。しかも、アームユニット座標系３１２から見たアームユニット制御点３２３の指令速度も時々刻々と変化する複雑な指令が必要となる。この場合、アームユニットコントローラにアームユニット制御点３２３の位置・姿勢、指令速度をNURBS曲線やベジエ曲線などの自由曲線で与える方法が考えられるが、自由曲線に対する補間処理は負荷が高く、特に多軸の制御ユニットの場合、制御周期内で補間処理が完了しないといった問題が発生する可能性が高くなる。
（２）の方法では、図３においては、移動ユニット３０１のコントローラからアームユニット３０２のコントローラへ、一定周期で制御点の位置データを送信する必要があり、通信負荷が大きくなるという課題がある。
本発明はこのような問題点に鑑みてなされたものであり、ユニットコントローラに対する指令を簡略化するとともに、ユニットコントローラ間の通信負荷を高めることなく、複数のユニットを協調して動作させることができる方法を提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記問題を解決するため、本発明は、次のようにしたのである。
請求項１乃至２に記載の発明は、ロボットを構成する２つの制御ユニットＡ、Ｂにおいて、制御ユニットＡの制御点ＡＣの移動に伴い、制御ユニットＢのローカル座標系であるユニット座標系Ｂの原点ＢＯが移動する場合に、ユニット座標系Ａにおける、制御ユニットＢの制御点ＢＣの目標位置、目標位置までの補間形状、指令速度に加えて、ユニット座標系Ａにおける、制御ユニットＢの原点ＢＯの現在位置、目標位置、目標位置までの補間形状、指令速度を入力して、制御周期ごとの、ユニット座標系Ｂにおける制御点ＢＣの補間位置を計算することで、アクチュエータの位置を計算し、アクチュエータを制御するという手順をとったのである。
また、請求項３乃至４に記載の発明は、２つの制御ユニット（制御ユニットＡおよび制御ユニットＢ）と、制御ユニットを統括する統括コントローラとをネットワークで接続して構成したロボットにおいて、制御ユニットＢに対して、ユニット座標系Ａにおける、制御ユニットＢのユニット座標系Ｂの原点ＢＯの補間指令も与え、制御ユニットＢのユニットコントローラＢは、ユニット座標系Ａにおける、制御点ＢＣの補間計算に加え、原点ＢＯの補間計算も実施することで、制御ユニットＢ自身のユニット座標系Ｂにおける制御点ＢＣの位置を計算し、アクチュエータを制御するものである。
【発明の効果】
【０００８】
請求項１乃至は２乃至は３に記載の発明によると、ロボットを構成する２つの制御ユニットＡ，Ｂにおいて、制御ユニットＡの制御点ＡＣの移動に伴い、制御ユニットＢ上のユニット座標系原点ＢＯが移動する場合に、ユニット座標系Ａから見て制御ユニットＢの制御点ＢＣも原点ＢＯも共にそれぞれの目標位置へ向かって、例えば直線移動する場合に、制御ユニットＢの原点ＢＯから見た制御点ＢＣの相対的な軌跡が図５に示すような自由曲線になったとしても、制御ユニットＢのユニットコントローラは、自身の制御点ＢＣの直線補間のモーション指令に加えて、制御ユニットＡの制御点ＡＣの直線補間のモーション指令を入力することで、制御周期ごとの制御点ＢＣ及び原点ＢＯの補間位置を計算することができ、原点ＢＯから見た制御点ＢＣの相対的な位置を制御周期ごとに計算することができるため、制御ユニットＢと共に制御ユニットＡが移動する場合であっても、制御ユニットＢの制御点ＢＣを所望の軌跡に沿って制御することができる。
また、請求項４に記載の発明によると、背景技術の項で説明した移動型双腕ロボットのように、移動ユニット上にアームユニットが設置されている場合にも、移動型双腕ロボットの統括コントローラから、アームユニットには、ユニット座標系Ａ上のアームユニットの制御点に対する指令に加えて、移動ユニットの制御点に対する指令も与えることができるので、移動ユニットとアームユニット間で制御周期ごとの位置データを送受信する必要はなく、通信負荷を高めることにならない。
【図面の簡単な説明】
【０００９】
【図１】本発明の方法を適用するロボットの構成を示すブロック図
【図２】本発明の方法の処理手順を示すフローチャート
【図３】本発明の適用対象となる移動型アームロボットの模式図
【図４】本発明の適用対象となる移動型アームロボットの動作例
【図５】図４の動作例における従来方法（１）の問題点
【図６】従来方法（２）の処理概要を示す模式図
【図７】移動型双腕ロボットの例
【発明を実施するための形態】
【００１０】
以下、本発明の方法の具体的実施例について、図に基づいて説明する。
【実施例１】
【００１１】
図１は、本発明の方法を実施する移動型アームロボットの構成を示すブロック図である。移動型アームロボットの概要は図３に基づいて上述した通りであるので、図3の説明は省略する。なお、図3にはエンドエフェクタユニット３０３が記載されているが、本実施例の説明においては不要であるため、図１から省いている。
図１において、移動型アームロボットは、移動ユニット１２０とアームユニット１３０、およびこれらのユニットを統括制御する統括コントローラ１１０から構成されている。また、移動ユニット１２０はユニットコントローラ１２１およびアクチュエータ１２２から、アームユニット１３０はユニットコントローラ１３１とアクチュエータ１３２とから構成されており、統括コントローラ１１０と、移動ユニットのユニットコントローラ１２１、アームユニットのユニットコントローラ１３１は通信線１４０で接続されている。
【００１２】
移動型アームロボット１００は、予めユーザによって教示され、統括コントローラ１１０上に保存された、アームユニット制御点３２３に対する移動ユニット座標系３１１上のモーション指令１１２を入力すると、指令生成部１１１にて、移動ユニット１２０とアームユニット１３０に対するモーション指令にそれぞれ分割する。例えば、図4に示すように、アームユニット制御点３２３に対する移動ユニット座標系３１１上のモーション指令が直線補間指令、移動ユニット制御点３２２に対する移動ユニット座標系３１１上のモーション指令が円弧補間指令の場合には、移動ユニット１２０およびアームユニット１３０に対するそれぞれのモーション指令moveCircular 、moveLinearTravelはそれぞれ以下のようなものとなる。
moveCircular：
moveCircular(
移動ユニット制御点３２２の移動ユニット座標系３１１上の円弧中心位置、
移動ユニット制御点３２２の移動ユニット座標系３１１上の目標位置と姿勢、
移動ユニット制御点３２２の指令速度
）
moveLinearTravel：
moveLinearTravel(
アームユニット制御点３２３の移動ユニット座標系３１１上の目標位置と姿勢、
アームユニット制御点３２３の指令速度
アームユニット原点３２２の補間種別（ここでは「Circular」）、
アームユニット原点３２２の移動ユニット座標系３１１上の円弧中心位置、
アームユニット原点３２２の移動ユニット座標系３１１上の現在位置と姿勢、
アームユニット原点３２２の移動ユニット座標系３１１上の目標位置と姿勢、
アームユニット原点３２２の指令速度、
)
【００１３】
次に、これらのモーション指令を入力する移動ユニット１２０、アームユニット１３０の処理概要について説明する。
移動ユニット１２０のユニットコントローラ１２１の補間指令入力部１２３は、統括コントローラ１１０より出力された上述のモーション指令moveCircularを入力し、移動ユニット制御点３２２に対するモーション指令を出力する。移動ユニット制御点３２２に対するモーション指令は、制御点補間処理部１２６により、制御周期ごとの、移動ユニット座標系３１１上の移動ユニット制御点３２２の位置・姿勢ＯＣ１に変換される。アクチュエータ補間処理部１２７は移動ユニット制御点３２２の移動ユニット座標系３１１上の位置・姿勢ＯＣ１を入力し、移動ユニット１２０の機構に応じた逆変換処理を施し、アクチュエータ１２２への指令データに変換する。この指令データをアクチュエータ制御部１２８が入力し、アクチュエータ１２２を制御することになる。
【００１４】
アームユニット１３０のユニットコントローラ１３１の補間指令入力部１３３は、統括コントローラ１１０より出力された上述のモーション指令moveLinearTravelを入力し、アームユニット座標系原点３２２に対するモーション指令と、アームユニット制御点３２３に対するモーション指令を出力する。アームユニット座標系原点３２２に対するモーション指令は、移動ユニット座標系原点補間処理部１３４により、制御周期ごとの補間点に分割され、移動ユニット座標系３１１上のアームユニット座標系原点３２２の位置・姿勢Ｏ２として出力される。一方、アームユニット制御点３２３に対するモーション指令は、移動ユニット座標系制御点補間処理部１３５により、制御周期ごとの補間点に分割され、移動ユニット座標系３１１上のアームユニット制御点３２３の位置・姿勢Ｃ２として出力される。アームユニット座標系制御点補間処理部１３６は、上述の原点位置・姿勢Ｏ２および制御点位置・姿勢Ｃ２を入力し、アームユニット１３０が持つローカル座標系であるアームユニット座標系３１２上のアームユニット制御点３２３の位置・姿勢ＯＣ２に変換する。アームユニット座標系原点３２２、およびアームユニット制御点３２３の移動ユニット座標系３１１上の位置・姿勢を表す同時変換行列をそれぞれＴＯ２、ＴＯＣ２とし、アームユニット制御点３２３のユニット座標系３１２上の位置・姿勢を表す同時変換行列をＴＵＣ２とすると、
ＴＵＣ２＝（ＴＯ２）−１ＴＯＣ２
となるので、この同時変換行列ＴＵＣ２に従って、アームユニット制御点３２３のアームユニット座標系３１２上の位置・姿勢ＯＣ２を計算する。
アクチュエータ補間処理部１３７はアームユニット制御点３２３のアームユニット座標系３１２上の位置・姿勢ＯＣ２を入力し、アームユニット１３０の機構に応じた逆変換処理を施し、アクチュエータ１３２への指令データに変換する。この指令データをアクチュエータ制御部１３８が入力し、アクチュエータ１３２を制御することになる。
また、ユニットコントローラ１２１、１３１は互いに同期するための同期制御部１２９、１３９を備えている。これらの同期制御部１２９、１３９は、通信線１４０を介して、移動ユニット１２０の補間処理部（制御点補間処理部１２６およびアクチュエータ補間処理部１２７）と、アームユニット１３０の補間処理部（移動ユニット座標系原点補間処理部１３４、移動ユニット座標系制御点補間処理部１３５、アームユニット座標系制御点補間処理部１３６およびアクチュエータ補間処理部１３７）との同期のタイミングを調整する。この手段として、通信線１４０に同期通信プロトコルを使用することもできるし、非同期通信であってもＩＥＥＥ１５８８のような時刻同期プロトコルによる同期も可能である。
【００１５】
このように、移動ユニット１２０上に設置されたアームユニット１３０のユニットコントローラ１３１が、アームユニット座標系原点３２２のモーション指令の処理部と、ユニットコントローラ１２１、１３１の処理を同期させる同期処理部１２９、１３９とをそれぞれ備えており、
移動ユニット１２０とアームユニット１３０が同期して、移動ユニット制御点３２２に対する移動ユニット座標系３１１上の位置・姿勢と、アームユニット制御点３２３に対するアームユニット座標系３１２上の位置・姿勢を計算することができるため、アームユニット制御点３２３に対する移動ユニット座標系３１１上の位置・姿勢を計算することができる。
【００１６】
図２はアームユニット１３０に内蔵されたユニットコントローラ１３１において本発明の方法の処理手順を示すフローチャートである。この図を用いて本発明の方法を順を追って説明する。
【００１７】
はじめにステップＳ２００で、アームユニット制御点３２３に対する移動ユニット座標系３１１上の目標位置、目標位置までの補間形状、指令速度を入力する。本実施例の場合、上述したmoveLinearTravel()となる。
次に、ステップＳ２０１で、アームユニット座標系原点３２２に対する移動ユニット座標系３１１上の現在位置・姿勢と、目標位置と姿勢と、目標位置までの補間形状、指令速度を入力する。
ステップＳ２０２では、補間計算が完了したか否かを判定し、判定結果がYesならば、補間処理が完了となる。一方、ステップＳ２０２の判定結果がNoならば、ステップＳ２０３へ移行する。
ステップＳ２０３では、アームユニット制御点３２３に対する、移動ユニット座標系３１１上の補間位置を補間周期ごとに計算し、続くステップＳ２０４では、アームユニット座標系原点３２２に対する、移動ユニット座標系３１１上の補間位置を補間周期ごとに計算する。
ステップＳ２０５では、ステップＳ２０３とステップＳ２０４の結果出力される補間位置から、アームユニット制御点３２３に対する、アームユニット座標系３１２上の補間位置を計算する。具体的には、上述したように、アームユニット座標系原点３２２、およびアームユニット制御点３２３の移動ユニット座標系３１１上の位置・姿勢を表す同時変換行列をそれぞれＴＯ２、ＴＯＣ２とし、アームユニット制御点３２３のユニット座標系３１２上の位置・姿勢を表す同時変換行列をＴＵＣ２とすると、
ＴＵＣ２＝（ＴＯ２）−１ＴＯＣ２
となるので、この同時変換行列ＴＵＣ２に従って、アームユニット制御点３２３のアームユニット座標系３１２上の位置・姿勢ＯＣ２が計算できる。
最後に、ステップＳ２０６では、アームユニット制御点３２３のアームユニット座標系３１２上の位置・姿勢ＯＣ２に対して、アームユニット１３０の機構に応じた逆変換処理を施し、アクチュエータ１３２の補間位置を制御周期ごとに計算し、ステップＳ２０７では、アクチュエータ１３２を制御周期ごとに制御する。
【００１８】
このように、アームユニット１３０のユニットコントローラ１３１において、アームユニット座標系原点３２２の移動を考慮した処理ができるので、アームユニット座標系原点３２２が移動ユニット１２０上に設置され、移動ユニット１２０が移動する場合においても、移動ユニット座標系３１１から見て、アームユニット制御点３２３を所望の軌跡に沿って制御することができる。
なお、移動ユニット１２０の場合は、図２に示したアームユニット１３０の処理手順のうち、移動ユニット座標系原点が移動することを考慮した処理手順Ｓ２０１およびＳ２０３およびＳ２０４をスキップした手順となる。
【符号の説明】
【００１９】
１００、３００  移動型アームロボット
１１０          統括コントローラ
１１１          指令生成部
１１２          モーション指令
１２０、３０１  移動ユニット
１２１、１３１  ユニットコントローラ
１２２、１３２  アクチュエータ
１２３、１３３  補間指令入力部
１３４          移動ユニット座標系原点補間処理部
１３５          移動ユニット座標系制御点補間処理部
１２６、１３６  アームユニット座標系制御点補間処理部
１２７、１３７  アクチュエータ補間処理部
１２８、１３８  アクチュエータ制御部
１３０、３０２  アームユニット
１４０          通信線
３０３          エンドエフェクタユニット
３１１          移動ユニット座標系
３１２          アームユニット座標系
３１３          エンドエフェクタユニット座標系
３２１          移動ユニット座標系原点
３２２          移動ユニット制御点、またはアームユニット座標系原点
３２３          アームユニット制御点、またはエンドエフェクタユニット座標系原点
６０１、６０２  アームユニット
６１１、６１２  アームユニットコントローラ
６２１、６２２  アームユニット制御点
７００          移動型双腕ロボット
７０１          移動機構
７０２          右エンドエフェクタ
７０３          左エンドエフェクタ
７０４          右腕
７０５          左腕
【図１】
2012045636.tif 000003
【図２】
2012045636.tif 000004
【図３】
2012045636.tif 000005
【図４】
2012045636.tif 000006
【図５】
2012045636.tif 000007
【図６】
2012045636.tif 000008
【図７】
2012045636.tif 000009