JP2004094399A

JP2004094399A - 多関節マニピュレータの制御方法及びその制御プログラム、並びにその制御システム

Info

Publication number: JP2004094399A
Application number: JP2002251907A
Authority: JP
Inventors: Noriko Onishi; 大西　典子; Hitoshi Miyauchi; 宮内　均; Tadashi Murata; 村田　直史
Original assignee: Mitsubishi Heavy Industries Ltd
Current assignee: Mitsubishi Heavy Industries Ltd
Priority date: 2002-08-29
Filing date: 2002-08-29
Publication date: 2004-03-25

Abstract

【課題】ＮＣデータをそのまま使用して多関節マニピュレータを制御すること。
【解決手段】マニピュレータ先端部が対象物を切削する際の走査軌道を決定する（ステップＳ１０１）。次に、この走査軌道を３次元ＣＡＤで作られた加工モデルに投影して、マニピュレータ先端部が対象物を切削するための走査軌道の点列データを取得する（ステップＳ１０２）。次に、取得した走査軌道の点列データから、マニピュレータ先端部の先端位置姿勢行列Ｔを、それぞれの走査軌道の点列データに対して求める（ステップＳ１０３）。このようにして求めた上記走査軌道Ｒ１に対応する各先端位置姿勢行列Ｔを、多関節マニピュレータの制御装置が取得する（ステップＳ１０４）。そして、マニピュレータ先端部を３次元ＣＡＤで作られた加工モデルの表面に対応した所定の軌道で動作（ステップＳ１０５）させて、仮想形状モデルである加工モデルの形状に加工対象物を切削する。
【選択図】　　　図２

Description

【０００１】
【発明の属する技術分野】
この発明は、多関節マニピュレータの制御に関し、さらに詳しくは、ＮＣデータをそのまま使用して多関節マニピュレータを制御する多関節マニピュレータの制御方法及びその制御プログラム、並びにその制御システムに関する。
【０００２】
【従来の技術】
多関節マニピュレータは、溶接、部品の取付け、塗装等に使用することができ、自動車や電子機器の製造、組立ての分野で広く使用される。その他にも、近年では原子炉内や水中等といった人間が作業するには過酷な条件下での作業においても使用されており、その応用範囲は広がっている。このようなマニピュレータは、６軸又は７軸の可動軸を持ち、予め用意した軌道データによって、マニピュレータの先端部がこの軌道を通るように制御される。
【０００３】
【発明が解決しようとする課題】
ところで、従来の多関節マニピュレータを制御する場合には、予め用意した先端部の軌道データを、多関節マニピュレータを動作させることのできるロボット言語に変換する必要があった。このため、ＣＡＤ（Ｃｏｍｐｕｔｅｒ　Ａｉｄｅｄ　Ｄｅｓｉｇｎ）上で生成されたＮＣ（Ｎｕｍｅｒｉｃａｌ　Ｃｏｎｔｒｏｌ：数値制御）データが得られても、これをロボット言語に変換する手順が必要で、制御データの作成に大幅な時間を要しており、多関節マニピュレータの制御には手間を要していた。
【０００４】
そこで、この発明は、上記に鑑みてなされたものであって、３次元ＣＡＤモデルのような仮想モデルから生成されたＮＣデータをそのまま使用して多関節マニピュレータを制御できる多関節マニピュレータの制御方法及びその制御プログラム、並びにその制御システムを提供することを目的とする。
【０００５】
【課題を解決するための手段】
上述の目的を達成するために、請求項１に係る本発明は、多関節マニピュレータを制御するにあたり、仮想形状モデル表面に前記多関節マニピュレータの先端部の走査軌道を投影して、前記仮想形状モデル表面上に前記先端部の走査軌道点列を形成する手順と、前記走査軌道点列を構成する各走査軌道点の位置座標と法線ベクトルとを求める手順と、前記走査軌道点の位置座標を前記多関節マニピュレータ側の座標系における位置座標に変換して前記先端部の位置を求め、さらに前記仮想形状モデルの座標系における重力方向のベクトルと前記法線ベクトルとから前記先端部の姿勢を求めることにより、前記多関節マニピュレータの先端位置姿勢行列Ｔを決定する手順と、前記先端位置姿勢行列Ｔを目標値として前記先端部を制御する手順とを含む多関節マニピュレータの制御方法であることを特徴とする。
【０００６】
また、請求項３に係る本発明は、多関節マニピュレータを制御するにあたり、仮想形状モデル表面に前記多関節マニピュレータの先端部の走査軌道を投影して、前記仮想形状モデル表面上に前記先端部の走査軌道点列を形成する手順と、前記走査軌道点列を構成する各走査軌道点の位置座標と法線ベクトルとを求める手順と、前記走査軌道点の位置座標を前記多関節マニピュレータ側の座標系における位置座標に変換して前記先端部の位置を求め、さらに前記法線ベクトルと前記仮想形状モデルの座標系における重力方向のベクトルとから前記先端部の姿勢を求めることにより、前記多関節マニピュレータの先端位置姿勢行列Ｔを決定する手順とをコンピュータに実行させ、前記多関節マニピュレータの先端部の走査軌道点列情報を生成させる多関節マニピュレータの制御プログラムであることを特徴とする。
【０００７】
また、請求項５に係る本発明は、仮想形状モデル表面に多関節マニピュレータの先端部の走査軌道を投影して、前記仮想形状モデル表面上に前記先端部の軌道点列を形成する手順と、前記走査軌道点列を構成する各走査軌道点の位置座標と法線ベクトルとを求める手順と、前記走査軌道点の位置座標を前記多関節マニピュレータ側の座標系における位置座標に変換して前記先端部の位置を求め、さらに前記法線ベクトルと前記仮想形状モデルの座標系における重力方向のベクトルとから前記先端部の姿勢を求める手順とにより、前記多関節マニピュレータの先端位置姿勢行列Ｔを決定する先端位置姿勢決定手段と、前記先端位置姿勢行列Ｔを目標値として前記先端部を制御する先端位置制御手段とを有する多関節マニピュレータ制御システムであることを特徴とする。
【０００８】
この多関節マニピュレータの制御方法、制御プログラム又は制御システムは、ＮＣデータで与えられる仮想モデル上における多関節マニピュレータ先端部の走査軌道データから直接多関節マニピュレータの先端位置姿勢行列Ｔを決定し、これを目標値として多関節マニピュレータ先端部を制御する。このため、従来必要であったＮＣデータをロボット言語に変換する手順を省略できるので、多関節マニピュレータの制御データを作る時間が大幅に短縮できる。また、ロボット言語を意識しないで、多関節マニピュレータ先端部の制御ができるので、専門のオペレータでなくとも容易に多関節マニピュレータを操作できる。
【０００９】
また、従来のように、多関節マニュピレータ制御用のＮＣデータをロボット言語に変換していた場合には、実際の動作前にマニピュレータの干渉を確認するシミュレーションが必要であった。そして、シミュレーションによってマニピュレータの干渉が判明した場合には、制御用のＮＣデータを作り直して干渉を回避していた。この作業時には、ＮＣデータを作り変える毎にロボット言語への変換が必要であったので、実際の作業に入るまで多大な手間を要していた。しかし、本発明においては、制御用ＮＣデータによって多関節マニピュレータを直接制御できるので、データを作り直してもすぐ動作確認ができ、シミュレーションによる干渉回避の手間を大幅に軽減できる。なお、この発明に使用する多関節マニピュレータは、制御に対する汎用性の高いオープンアーキテクチャタイプの多関節マニピュレータが好ましい（以下同様）。
【００１０】
また、請求項２に係る本発明は、上記多関節マニピュレータの制御方法において、上記多関節マニピュレータは冗長マニピュレータであり、上記多関節マニピュレータの腕部が移動する方向に障害物がある場合には、さらに、上記多関節マニピュレータの腕部を構成する軸の一つを冗長軸として決定する手順と、当該冗長軸を動かす所定の角度及びその大きさを決定する指定する手順と、この所定の角度及び大きさから評価関数を定義する手順と、当該評価関数に基づいて前記冗長軸を動かして、上記先端部の位置を上記目標値に保ったまま前記障害物を回避させる手順とを含むことを特徴とする。
【００１１】
また、請求項４に係る本発明は、上記多関節マニピュレータの制御プログラムにおいて、上記多関節マニピュレータは冗長マニピュレータであり、上記多関節マニピュレータの腕部が移動する方向に障害物がある場合には、さらに、コンピュータ外部からの入力によって上記多関節マニピュレータの腕部を構成する軸の一つを冗長軸として決定する手順と、当該冗長軸を動かす所定の角度及びその大きさを決定する指定する手順と、この所定の角度及び大きさから評価関数を定義する手順と、当該評価関数に基づいて前記冗長軸を動かして、上記先端部の位置を上記目標値に保ったまま前記障害物を回避させる手順とを含むことを特徴とする。
【００１２】
このように、冗長マニピュレータを制御対象とする場合には、冗長軸を利用して障害物に対する干渉を回避できる。このため、障害物の位置が予め分かっていれば、その位置を避けるように制御することで、シミュレーションが不要あるいはその手間を大幅に軽減できる。ここで、冗長マニピュレータとしては、７軸マニピュレータを用い、人間の腕の冗長性を持たせることが好ましい。
【００１３】
【発明の実施の形態】
以下、この発明につき図面を参照しつつ詳細に説明する。なお、この実施の形態によりこの発明が限定されるものではない。また、下記実施の形態における構成要素には、当業者が容易に想定できるもの或いは実質的に同一のものが含まれる。
【００１４】
（実施の形態１）
図１は、この発明の実施の形態１に係る制御方法の制御対象である多関節マニピュレータを示す全体図である。同図に示す多関節マニピュレータ１０は、７個の関節を持つ７軸冗長マニピュレータである。各軸Ａ１〜Ａ７は、それぞれ図１（ａ）に示すように回転して、マニピュレータ先端部１２のロール、ヨー、ピッチ及び位置（Ｘ、Ｙ、Ｚ）を制御する。なお、６軸あれば、マニピュレータ先端部１２の位置と角度とを自由に制御できるが、この多関節マニピュレータ１０のように、冗長軸としてさらに１軸を設けることによって、マニピュレータ先端部１２の位置・姿勢を制御しつつ、腕部１４が障害物を避けるように制御することもできる。
【００１５】
この多関節マニピュレータ１０には、制御手段である制御装置３０が備えられている。制御装置３０は、後述するＮＣデータ変換によって得られた多関節マニピュレータ１０の先端位置姿勢行列であるＴ行列のデータを取得して、このＴ行列のデータに基づきサーボドライバ２０を介し、マニピュレータ先端部１２の軌道を制御する。なお、サーボドライバ２０は、多関節マニピュレータ１０に内蔵してもよい。
【００１６】
図１（ｂ）及び（ｃ）に示すように、この多関節マニピュレータ１０の第７軸Ａ７には、ドリル１６やエンドミル等の切削工具、溶接トーチ、塗装用スプレー１７、計測用のセンサあるいはプローブ等、各種のアタッチメントが取り付けられる。そして、取り付けたアタッチメントによって、対象物を切削加工や溶接したり、塗装したり、あるいは対象物の検査をすることができる。いずれの場合においても、加工モデルの表面に沿った軌道や溶接や塗装等に必要な軌道でマニピュレータ先端部１２（例えばドリル１６の先端部）を動作させる必要があるので、このような軌道データが必要になる。次に、ドリル１６によって対象物を所定の形状に切削加工する場合を例にとって説明する。なお、本発明の適用対象はドリルによる切削加工に限られず、溶接、塗装、研磨、検査、あるいはペンによる作画等に対しても本発明は適用できる。
【００１７】
図２は、この発明の実施の形態１に係る多関節マニピュレータの制御方法の概要を示すフローチャートである。この多関節マニピュレータの制御方法は、３次元ＣＡＤモデル等の仮想形状モデルから作成されたＮＣデータをそのまま使用して、多関節マニピュレータ先端の走査軌道を直接制御する点に特徴がある。図２を用いて、この発明の実施の形態１に係る多関節マニピュレータの制御方法の概要を説明する。まず、マニピュレータ先端部１２が対象物を切削する際の走査軌道Ｒ０を決定する（ステップＳ１０１）。
【００１８】
次に、この走査軌道Ｒ０を３次元ＣＡＤで作られた加工モデルに投影して、マニピュレータ先端部１２が対象物を切削するための走査軌道Ｒ１の点列データを取得する（ステップＳ１０２）。なお、この走査軌道点列データは、ｎ個の点が集まったものであり、各走査軌道点のデータは、前記３次元ＣＡＤの加工モデル面上における位置座標Ｕ（Ｘ、Ｙ、Ｚ）及び面上に対して垂直となる法線ベクトルＶで構成される。以下、走査軌道点データは（Ｘ、Ｙ、Ｚ、Ｖ）で表す。また、この各走査軌道点データは、ＮＣ用データのフォーマットで与えられる。
【００１９】
次に、取得した走査軌道Ｒ１の点列データから、マニピュレータ先端部１２の先端位置姿勢行列Ｔをそれぞれの走査軌道Ｒ１の点列データに対して求める（ステップＳ１０３）。このようにして求めた上記走査軌道Ｒ１に対応する各先端位置姿勢行列Ｔを、多関節マニピュレータ１０の制御装置３０が取得する（ステップＳ１０４）。そして、マニピュレータ先端部１２を３次元ＣＡＤで作られた加工モデルの表面に対応した所定の軌道で動作（ステップＳ１０５）させて、仮想形状モデルである加工モデルの形状に加工対象物を切削する。なお、この発明に係る多関節マニピュレータの制御方法に使用する多関節マニピュレータ１０は、より汎用性の高い階層の制御コード（例えばＣ言語）を使用できる、制御に対する汎用性の高いオープンアーキテクチャタイプの多関節マニピュレータが好ましい。
【００２０】
次に、３次元ＣＡＤの加工モデル面上におけるマニピュレータ先端部１２の走査軌道点列データを取得する手順、及びこの走査軌道点列データを構成する各走査軌道点Ｐ（Ｘ、Ｙ、Ｚ、Ｖ）から各先端位置姿勢行列Ｔを取得する手順について説明する。まず、前者について説明する。図３は、３次元ＣＡＤの加工モデルからマニピュレータ先端部の走査軌道点列データを取得する手順を示す説明図である。
【００２１】
図３（ａ）に示すように、仮想モデルである３次元ＣＡＤの加工モデル（以下加工モデル）４０に対して、例えばＸ−Ｙ平面状に、マニピュレータ先端部１２の走査軌道Ｒ０を現す連続線４２を描く。この走査軌道Ｒ０は、切削加工に適した軌道を任意に選択することができる。次に、この連続線４２を加工モデル４０に投影して、図３（ｂ）に示すような投影後の走査軌道Ｒ１各点における位置座標Ｕ（Ｘ、Ｙ、Ｚ）と法線ベクトルＶ（Ｉ、Ｊ、Ｋ）とを取得する。これらが走査軌道点データ（Ｘ、Ｙ、Ｚ、Ｖ）である。すなわち、加工モデル４０に投影した後における走査軌道Ｒ１の走査軌道点データ（Ｘ、Ｙ、Ｚ、Ｖ）が、マニピュレータ先端部１２が対象物を切削するための走査軌道点列データとなる。
【００２２】
次に、走査軌道点列データを構成する各走査軌道点データ（Ｘ、Ｙ、Ｚ、Ｖ）から各先端位置姿勢行列Ｔを取得する手順について説明する。図４は、多関節マニピュレータの座標系と３次元ＣＡＤの加工モデルの座標系とを示す説明図である。また、図５は、先端位置姿勢行列Ｔ行列を作成する手順を示す説明図である。図４（ａ）に示すように、多関節マニピュレータ１０の座標系では、多関節マニピュレータ１０のベース１０Ｂを原点として垂直方向がＺ軸をとり、Ｚ軸と互いに直交する軸がＸ軸及びＹ軸となる。これに対して、図４（ｂ）に示すように、切削対象である３次元ＣＡＤの加工モデル４０の座標系では、加工モデル４０のある軸４０ａと一致する軸がＺ軸である。そして、Ｚ軸と互いに直交する軸がＸ軸及びＹ軸となる。
【００２３】
このように、多関節マニピュレータ１０と加工モデル４０との座標系は異なるので、加工モデル４０の座標系における、ある走査軌道点の位置座標（Ｘ、Ｙ、Ｚ）の値を、多関節マニピュレータ１０の座標系においてマニピュレータ先端部１２の位置座標として使用しても両者は一致しない。したがって、両者を一致させなければ、各走査軌道点データを使用して多関節マニピュレータ１０に加工モデル４０の形状に対象物を切削させることはできない。このため、加工モデル４０の座標系における、ある走査軌道点の位置座標Ｕ（Ｘ、Ｙ、Ｚ）の値を、多関節マニピュレータ１０の座標系における位置座標Ｐ（Ｐ_Ｘ、Ｐ_Ｙ、Ｐ_Ｚ）に変換する。これは、例えば、加工モデル４０の座標系における原点と多関節マニピュレータ１０の座標系における原点との差を、ある軌道点の位置座標Ｕ（Ｘ、Ｙ、Ｚ）の値に加算又は減算すればよい。このようにして得た位置座標Ｐ（Ｐ_Ｘ、Ｐ_Ｙ、Ｐ_Ｚ）が、後述する先端位置姿勢行列Ｔの位置情報を表す。
【００２４】
ＮＣ用データには、加工モデル４０に対して、加工ツールであるドリル１６をどのような角度で当てるかという情報が、切削角度のベクトルＤ（Ｄ_Ｘ、Ｄ_Ｙ、Ｄ_Ｚ）として与えられている。多関節マニピュレータ１０の座標系においてこのベクトルＤ（Ｄ_Ｘ、Ｄ_Ｙ、Ｄ_Ｚ）をそのまま使用しても、上記理由から多関節マニピュレータ１０は加工モデル４０を切削することができない。したがって、同じようにベクトルＤ（Ｄ_Ｘ、Ｄ_Ｙ、Ｄ_Ｚ）をＡ（Ａ_Ｘ、Ａ_Ｙ、Ａ_Ｚ）に変換する。このベクトルＡは、マニピュレータ先端部１２におけるＺ方向のベクトルとなる。なお、切削角度のベクトルには、法線ベクトルＶ（Ｉ、Ｊ、Ｋ）を使用してもよい。
【００２５】
加工モデル４０の切削においては、マニピュレータ先端部１２におけるｚ方向のベクトルさえ決まれば、ｘ方向とｙ方向とのベクトルを決定しなくとも、マニピュレータ先端部１２の姿勢が決まるので、対象物を切削できる。しかしながら、マニピュレータ先端部１２の制御においては、マニピュレータ先端部１２のｘ方向とｙ方向とのベクトルも存在しないと制御できないため、これらのベクトルを新たに作り出す必要がある。この手順を、図５を用いて説明する。
【００２６】
マニピュレータ先端部１２のｘ方向とｙ方向とのベクトルは、任意のベクトルで構わない。本発明においては加工モデル４０の座標系における、ある走査軌道点の法線ベクトルＶ（Ｉ、Ｊ、Ｋ）と重力方向のベクトルとを利用してこれらのベクトルを作成する。まず、法線ベクトルＶと加工モデル座標系における−Ｚ方向（重力方向）の単位ベクトルＥ（０、０、１）との外積を求める。このようにして得たベクトルを、マニピュレータ先端部１２におけるｙ方向のベクトルＯ（Ｏ_Ｘ、Ｏ_Ｙ、Ｏ_Ｚ）とする。そして、このｙ方向のベクトルと法線ベクトルＶとの外積を求めることによって得たベクトルを、マニピュレータ先端部１２におけるｘ方向のベクトルＮ（Ｎ_Ｘ、Ｎ_Ｙ、Ｎ_Ｚ）とする。
【００２７】
このようにして求めたマニピュレータ先端部１２のｘ、ｙ及びｚ方向のベクトルと、多関節マニピュレータ１０の座標系における位置座標（Ｐ_Ｘ、Ｐ_Ｙ、Ｐ_Ｚ）とによって、マニピュレータの先端位置姿勢行列Ｔを表すことができる。ここで、マニピュレータの先端位置姿勢行列Ｔは、
【数１】

である。
【００２８】
このマニピュレータの先端位置姿勢行列Ｔは、加工モデル４０の表面にマニピュレータ先端部１２の走査軌道Ｒ０を投影した軌道Ｒ１の軌道点（Ｘ、Ｙ、Ｚ、Ｖ）の数だけ存在する。すなわち、走査軌道Ｒ１の走査軌道点列データと同じ数だけ存在する先端位置姿勢行列Ｔによって、マニピュレータ先端部１２の位置と姿勢とが決定される。これによって、マニピュレータ先端部１２は多関節マニピュレータ１０の座標系において、走査軌道Ｒ１に沿って移動するので、加工対象物を加工モデル４０の形状に切削できる。
【００２９】
加工対象物を切削する際には、上記算出手順を達成できるプログラムをパーソナル・コンピュータやワークステーション等で実行し、上記手順によって求められた先端位置姿勢行列Ｔを制御装置３０に読み込む。制御装置３０は、この先端位置姿勢行列Ｔを目標値として、サーボドライバ２０を介して多関節マニピュレータ１０の各軸Ａ１〜Ａ７を制御し、マニピュレータ先端部１２を軌道Ｒ１に沿って移動させる。このとき、マニピュレータ先端部１２の現在位置をフィードバックして、常に現在位置と目標値との偏差が０となるように制御される。
【００３０】
図６は、このフィードバック制御を示す説明図である。制御装置３０内には、フィードバック手段５０が備えられており、各軸Ａ１〜Ａ７（図１（ａ）参照）に設けられた角度センサ（図示せず）から出力される回転角度θ_１〜θ_７が入力される。この回転角度θ_１〜θ_７は、先端位置姿勢座標変換手段５４によって、マニピュレータ先端部１２の現先端在位置姿勢行列Ｔｐに変換される。位置制御ゲイン５１は、現先端在位置姿勢行列Ｔｐと目標先端位置姿勢行列Ｔｒとの偏差ｅｐを増幅し、先端位置姿勢速度Ｖｐｗを出力する。また、ヤコビ行列・擬似ヤコビ行列生成手段５３では、前記回転角度θ_１〜θ_７が入力されて角度変換用ヤコビ行列Ｊを算出する。軸角速度変換手段５２は、先端位置姿勢速度Ｖｐｗと角度変換用ヤコビ行列Ｊとから、第（１）式に基づいて目標の各軸角速度Θ_１〜Θ_７を出力する。サーボドライバ２０は、この各軸角速度Θ_１〜Θ_７に基づいて各軸Ａ１〜Ａ７に設けられたアクチュエータ（図示せず）を駆動して、マニピュレータ先端部１２を目標位置に制御する。
Θ＝［Ｊ^＃］［Ｖｐｗ］＋（［Ｉ］−［Ｊ^＃］［Ｊ］）・δ（θ）・Ｋ・・・（１）
ここで、［Ｊ］は６×７ヤコビ行列、［Ｊ^＃］は７×６疑似逆ヤコビ行列、［Ｉ］は７×７単位行列、δ（θ）は７×１方向ベクトル、Ｋは任意のスカラー量である。
【００３１】
第（１）式の右辺第１項である［Ｊ^＃］［Ｖｐｗ］は特殊解と呼ばれ、マニピュレータ先端部１２を目標とする位置・姿勢に移動させるための最も移動量が小さい解である。これは、マニピュレータ先端部１２の先端位置姿勢速度Ｖｐｗによって一意に決まる。また、第（１）式の右辺第２項である（［Ｉ］−［Ｊ^＃］［Ｊ］）・δ（θ）・Ｋは同次項と呼ばれ、残された冗長性を制御する値である。この値を制御することによって、マニピュレータ先端部１２の位置・姿勢を変化させずに多関節マニピュレータ１０の姿勢を変化させることができる。同次項のうち、［Ｉ］−［Ｊ^＃］［Ｊ］はヤコビ行列の零空間であり一意に決まる。また、δ（θ）・Ｋは評価関数と呼ばれる部分で、任意の値を入れることができる。
【００３２】
例えば、マニピュレータ先端部１２の位置・姿勢をある目標値に制御している多関節マニピュレータ１０において、ある指定した軸の現在位置を中心とし、ある幅をもった評価関数を定義する。そして、その中心位置を現在位置から徐々に動かした方向にずらしながら、同じ幅を持った評価関数に定義し直す。このようにすると、評価関数の幅の中心となるように軸を動かそうとする働きによって、前記指定した軸は外部から指定された方向へ角度を変化させる。このとき、第（１）式の特殊解によって、マニピュレータ先端部１２の位置・姿勢は維持される。
【００３３】
多関節マニピュレータ１０の腕部１４が障害物と干渉する場合には、その障害物を避けるようにある軸を冗長軸として指定する。そして、この冗長軸を所定の角度及びその大きさに動かすように指定し、ここから評価関数を定義する。次に、この定義した評価関数を、ヤコビ行列・擬似ヤコビ行列生成手段５３に与えて、角度変換用ヤコビ行列Ｊを算出する。軸角速度変換手段５２は、先端位置姿勢速度Ｖｐｗと角度変換用ヤコビ行列Ｊとから、第（１）式に基づいて冗長軸の角速度Θを出力する。このようにして、マニピュレータ先端部１２の位置・姿勢を維持したまま、多関節マニピュレータ１０の腕部１４を動かして、障害物との干渉を回避することができる。
【００３４】
なお、実施の形態１に係る本発明の多関節マニピュレータの制御方法は、上記手順を実行するプログラムをパーソナル・コンピュータやワークステーションなどのコンピュータで実行することによって実現することができる。このプログラムは、インターネットなどのネットワークを介して配布することができる。また、このプログラムは、ハードディスク、フレキシブルディスク（ＦＤ）、ＣＤ−ＲＯＭ、ＭＯ、ＤＶＤなどのコンピュータで読み取り可能な記録媒体に記録され、コンピュータによって記録媒体から読み出されることによって実行することもできる。以下の実施の形態においても同様である。
【００３５】
このように、本発明においては、ＮＣデータで与えられるマニピュレータ先端部１２の走査軌道データから、直接多関節マニピュレータの先端位置姿勢行列Ｔを決定し、これを目標値として多関節マニピュレータ先端部を制御する。このため、従来必要であったＮＣデータをロボット言語に変換する手順を省略できるので、多関節マニピュレータの制御データを作る時間が大幅に短縮できる。また、ロボット言語を意識しないで、多関節マニピュレータ先端部の制御ができるので、専門のオペレータでなくとも容易に多関節マニピュレータを操作できる。
【００３６】
また、事前のシミュレーションおいても制御用ＮＣデータによって多関節マニピュレータを直接制御できるので、制御データを作り直してもすぐ動作確認ができ、シミュレーションの手間を大幅に軽減できる。さらに、７軸マニピュレータのような冗長マニピュレータを使用すれば、多関節マニピュレータ１０の腕部１４が障害物に干渉する場合であっても、障害物を回避するように評価関数を与えて冗長軸を制御することができる。これによってシミュレーションを省略でき、さらに、ロボット言語に変換せずにＮＣデータから直接多関節マニピュレータを制御できるので、実際の動作までに要する時間を大幅に短縮できる。
【００３７】
（実施の形態２）
実施の形態２では、本発明を多関節マニピュレータの制御システムに適用した例を説明する。図７は、この発明の実施の形態に係る多関節マニピュレータの制御システムを示す説明図である。この多関節マニピュレータの制御システム６０は、多関節マニピュレータの先端位置姿勢行列Ｔを決定する先端位置姿勢決定手段６２と、ここから送られる先端位置姿勢行列Ｔによって、マニピュレータ先端部１２の位置姿勢を制御する制御手段６４とで構成される。
【００３８】
図７（ａ）に示すように、先端位置姿勢決定手段６２は、処理部６２ａと記憶部６２ｂとを有している。記憶部６２ｂには、本発明に係る多関節マニピュレータの制御方法のうち、先端位置姿勢行列Ｔを求める手順が記載されたプログラムや、計算に必要な３次元ＣＡＤのモデルデータ等が記憶されている。また、記憶部６２ｂは、処理部６２ａから送られる処理途中のデータを一時的に記憶して、処理部６２ａの処理を補助する。
【００３９】
処理部６２ａは、前記プログラムやデータを適宜読み出し、また、計算途中のデータを記憶部６２ｂに保存しつつ先端位置姿勢行列Ｔを求め、多関節マニピュレータ１０の走査軌道点列情報を算出する。この走査軌道点列情報は、先端位置姿勢行列Ｔの集合として提供される。なお、図７（ｂ）に示す多関節マニピュレータの制御システム６０’のように、先端位置姿勢決定手段６２の代わりに、パーソナル・コンピュータやワークステーション等のコンピュータ６５を使用してもよい。
【００４０】
制御手段６４は、先端位置姿勢決定手段６２やコンピュータ６５で求められたマニピュレータ先端部１２の走査軌道点列情報（先端位置姿勢行列Ｔの集合）を取得する。そして、この走査軌道点列情報をマニピュレータ先端部１２の目標値として、これを所定の走査軌道に従って作動させる。制御手段６４には、マニピュレータ先端部１２の現実の位置がフィードバックされて、常に目標値との現在位置との偏差が０となるように制御される。
【００４１】
ここで、上記先端位置姿勢決定手段６２の記憶部６２ｂは、ハードディスク装置や光磁気ディスク装置、フラッシュメモリ等の不揮発性のメモリ（読み出しのみが可能な記憶媒体）や、ＲＡＭ（Ｒａｎｄｏｍ　Ａｃｃｅｓｓ　Ｍｅｍｏｒｙ）のような揮発性のメモリ、あるいはこれらの組合せにより構成されるものとする。また、この処理部６２ａは専用のハードウエアにより実現されるものであってもよい。さらに、この処理部６２ａはメモリ及びＣＰＵ（中央演算装置）により構成され、処理部６２ａの機能を実現するためのプログラム（図示省略）をメモリにロードして実行することによりその機能を実現させるものであってもよい。また、この先端位置姿勢決定手段６２には、周辺機器として入力装置、表示装置など（いずれも図示省略）が接続されるものとする。ここで、入力装置とはキーボード、マウス等の入力デバイスのことをいう。表示装置とはＣＲＴ（Ｃａｔｈｏｄｅ　Ｒａｙ　Ｔｕｂｅ）や液晶表示装置などのことをいう。
【００４２】
【発明の効果】
以上説明したように、本発明に係る多関節マニピュレータの制御方法、制御プログラム又は制御システム（請求項１、３、４）では、ＮＣデータで与えられる多関節マニピュレータ先端部の走査軌道データから直接決定した多関節マニピュレータの先端位置姿勢行列Ｔを目標値として、多関節マニピュレータ先端部を制御するようにした。このため、多関節マニピュレータの制御データを作る時間が大幅に短縮できる。また、ロボット言語を意識しないで、多関節マニピュレータ先端部の制御ができるので、専門のオペレータでなくとも容易に多関節マニピュレータを操作できる。
【００４３】
また、この発明に係る多関節マニピュレータの制御方法（請求項２、４）では、冗長マニピュレータを制御対象とする場合に、障害物の位置が予め分かっていれば、その位置を避けるように制御するようにした。これによって、マニピュレータの干渉シミュレーションが不要、あるいはシミュレーションの手間を大幅に軽減できる。
【図面の簡単な説明】
【図１】この発明実施の形態１に係る制御方法の制御対象である多関節マニピュレータを示す全体図である。
【図２】この発明の実施の形態１に係る多関節マニピュレータの制御方法の概要を示すフローチャートである。
【図３】３次元ＣＡＤの加工モデルからマニピュレータ先端部の走査軌道点列データを取得する手順を示す説明図である。
【図４】多関節マニピュレータの座標系と３次元ＣＡＤの加工モデルの座標系とを示す説明図である。
【図５】先端位置姿勢行列Ｔ行列を作成する手順を示す説明図である。
【図６】フィードバック制御を示す説明図である。
【図７】この発明の実施の形態に係る多関節マニピュレータの制御システムを示す説明図である。
【符号の説明】
１０　多関節マニピュレータ
１２　マニピュレータ先端部
１４　腕部
１６　ドリル
２０　サーボドライバ
３０　制御装置
４０　加工モデル
４２　連続線
５０　フィードバック手段
５１　位置制御ゲイン
５２　軸角速度変換手段
５３　ヤコビ行列・擬似ヤコビ行列生成手段
５４　先端位置姿勢座標変換手段
６０、６０’　　制御システム
６２　先端位置姿勢決定手段
６４　制御手段

Claims

多関節マニピュレータを制御するにあたり、
仮想形状モデル表面に前記多関節マニピュレータの先端部の走査軌道を投影して、前記仮想形状モデル表面上に前記先端部の走査軌道点列を形成する手順と、
前記走査軌道点列を構成する各走査軌道点の位置座標と法線ベクトルとを求める手順と、
前記走査軌道点の位置座標を前記多関節マニピュレータ側の座標系における位置座標に変換して前記先端部の位置を求め、さらに前記仮想形状モデルの座標系における重力方向のベクトルと前記法線ベクトルとから前記先端部の姿勢を求めることにより、前記多関節マニピュレータの先端位置姿勢行列Ｔを決定する手順と、
前記先端位置姿勢行列Ｔを目標値として前記先端部を制御する手順と、
を含むことを特徴とする多関節マニピュレータの制御方法。
上記多関節マニピュレータは冗長マニピュレータであり、上記多関節マニピュレータの腕部が移動する方向に障害物がある場合には、さらに、上記多関節マニピュレータの腕部を構成する軸の一つを冗長軸として決定する手順と、当該冗長軸を動かす所定の角度及びその大きさを決定する指定する手順と、この所定の角度及び大きさから評価関数を定義する手順と、当該評価関数に基づいて前記冗長軸を動かして、上記先端部の位置を上記目標値に保ったまま前記障害物を回避させる手順とを含むことを特徴とする請求項１に記載の多関節マニピュレータの制御方法。
多関節マニピュレータを制御するにあたり、
仮想形状モデル表面に前記多関節マニピュレータの先端部の走査軌道を投影して、前記仮想形状モデル表面上に前記先端部の走査軌道点列を形成する手順と、
前記走査軌道点列を構成する各走査軌道点の位置座標と法線ベクトルとを求める手順と、
前記走査軌道点の位置座標を前記多関節マニピュレータ側の座標系における位置座標に変換して前記先端部の位置を求め、さらに前記法線ベクトルと前記仮想形状モデルの座標系における重力方向のベクトルとから前記先端部の姿勢を求めることにより、前記多関節マニピュレータの先端位置姿勢行列Ｔを決定する手順と、
をコンピュータに実行させ、前記多関節マニピュレータの先端部の走査軌道点列情報を生成させることを特徴とする多関節マニピュレータの制御プログラム。
上記多関節マニピュレータは冗長マニピュレータであり、上記多関節マニピュレータの腕部が移動する方向に障害物がある場合には、さらに、コンピュータ外部からの入力によって上記多関節マニピュレータの腕部を構成する軸の一つを冗長軸として決定する手順と、当該冗長軸を動かす所定の角度及びその大きさを決定する指定する手順と、この所定の角度及び大きさから評価関数を定義する手順と、当該評価関数に基づいて前記冗長軸を動かして、上記先端部の位置を上記目標値に保ったまま前記障害物を回避させる手順とを含むことを特徴とする請求項３に記載の多関節マニピュレータの制御プログラム。
仮想形状モデル表面に多関節マニピュレータの先端部の走査軌道を投影して、前記仮想形状モデル表面上に前記先端部の軌道点列を形成する手順と、前記走査軌道点列を構成する各走査軌道点の位置座標と法線ベクトルとを求める手順と、前記走査軌道点の位置座標を前記多関節マニピュレータ側の座標系における位置座標に変換して前記先端部の位置を求め、さらに前記法線ベクトルと前記仮想形状モデルの座標系における重力方向のベクトルとから前記先端部の姿勢を求める手順とにより、前記多関節マニピュレータの先端位置姿勢行列Ｔを決定する先端位置姿勢決定手段と、
前記先端位置姿勢行列Ｔを目標値として前記先端部を制御する先端位置制御手段と、
を有することを特徴とする多関節マニピュレータの制御システム。