ロボット制御装置，ロボット制御方法，コンピュータプログラム
【課題】 ロボットが使用される状況に応じて要求される条件を考慮することで、アーム先端を目標位置に移動させるために必要な走行軸の移動量を自動的に決定することができるロボット制御装置を提供する。
【解決手段】 ロボットの制御を行うコントローラは、垂直多関節型で６軸のアームの先端について、現在位置（Ｘ０，Ｙ０，Ｚ０）と所定時間後の目標移動量（ΔＸ，ΔＹ，ΔＺ）とに基づいて決定される目標位置（Ｘｃ，Ｙｃ，Ｚｃ）に対し（ステップＳ１〜Ｓ４）、その時点でアームが保持している第２関節角度θ２と第３関節角度θ３との和が一定となることを拘束条件として、アームの先端を目標位置に移動させるために必要な走行軸上における所定時間当たりの移動量ΔＸ３を計算する（ステップＳ５）。
【解決手段】 ロボットの制御を行うコントローラは、垂直多関節型で６軸のアームの先端について、現在位置（Ｘ０，Ｙ０，Ｚ０）と所定時間後の目標移動量（ΔＸ，ΔＹ，ΔＺ）とに基づいて決定される目標位置（Ｘｃ，Ｙｃ，Ｚｃ）に対し（ステップＳ１〜Ｓ４）、その時点でアームが保持している第２関節角度θ２と第３関節角度θ３との和が一定となることを拘束条件として、アームの先端を目標位置に移動させるために必要な走行軸上における所定時間当たりの移動量ΔＸ３を計算する（ステップＳ５）。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、走行軸を冗長軸とし、垂直多関節型で６軸のアームを有するロボットの制御を行う制御装置及び制御方法、並びに前記制御装置を構成するコンピュータにより実行されるプログラムに関する。
【背景技術】
【０００２】
走行軸を冗長軸として、垂直多関節型で６軸のアームを有するロボットには、計７つの自由度が存在する。従って、上記ロボットを制御してアームの先端を目標位置まで移動させるには、それら７つの自由度を全て規定する必要があり、ティーチング作業やリアルタイム操作が煩雑であるという問題がある。
例えば、特許文献１には、上記と同様に構成されるロボットについて、冗長軸に関するティーチング時間を「０」とするようにしたプログラムの作成方法及び装置が開示されている。この技術は、冗長軸が取り得る位置を予め複数の離散的な位置に定めておき、オフラインティーチング時に得られるティーチングデータから、ティーチング時における複数の動作状態でのアーム先端部と冗長軸である走行台車の原点位置との距離を演算し、各演算結果に基づいて、動作プログラム中における走行台車の位置を予め定めた離散的な位置の何れかに自動的に決定する、というものである。
【特許文献１】特開平１０−２４４４８１号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、特許文献１の技術では、事前に冗長軸が取り得る位置を定める必要があり、しかも、実際の動作時におけるロボットの冗長軸上の位置は定められた何れかに限定されてしまう。従って、ロボットの制御を柔軟に行うことができないという問題がある。
本発明は上記事情に鑑みてなされたものであり、その目的は、ロボットが使用される状況に応じて要求される条件を考慮することで、アーム先端を目標位置に移動させるために必要な走行軸の移動量を自動的に決定することができるロボット制御装置及び制御方法、並びに前記制御装置を構成するコンピュータにより実行されるプログラムを提供することにある。
【課題を解決するための手段】
【０００４】
請求項１記載のロボット制御装置によれば、垂直多関節型で６軸のアームの先端について、現在位置と所定時間後の目標移動量とに基づいて決定される目標位置に対し、その時点でアームが保持している第２関節角度θ２と第３関節角度θ３との和が一定となることを拘束条件として、アームの先端を目標位置に移動させるために必要な走行軸上における所定時間当たりの移動量を計算する。
即ち、垂直多関節型で６軸のアームの先端を目標位置に移動させる場合には、現在位置として、アームの第１〜第３関節角度θ１〜θ３と、アームの原点から第２関節までの高さｄ２と、第２関節と第３関節との距離Ｌ２と、第３関節と前記アームの先端までの距離Ｌ３とがパラメータとして与えられる。また、第４〜第６関節の角度θ４〜θ６は、これらのパラメータと連動して、アーム先端に取り付けられている若しくはアーム先端のハンド等によって把持されている物体の姿勢を決定することになる。そして、上記のパラメータに加えて、アーム原点の走行軸上の移動量を決定することでアームの先端を目標位置に到達させる。
【０００５】
請求項２記載のロボット制御装置によれば、走行軸上における所定時間当たりの移動量に基づいて求めた移動速度が上限速度を超える場合には、拘束条件を解除して前記移動速度を上限速度に置き換える。そして、両速度の差に応じた移動量を走行軸上の座標値より減じて目標位置を修正する。即ち、請求項１のように設定した拘束条件に基づいて求めた移動量だけ、アームの先端を所定時間内に移動させようとすると、ロボットの動作条件として設定されている上限速度を超えてしまう場合が想定される。従って、上記のように修正を行えば、そのような事態の発生を回避することができる。
【発明を実施するための最良の形態】
【０００６】
（第１実施例）
以下、本発明をハンドリング作業用ロボットに適用した場合の第１実施例について図１乃至図６を参照して説明する。図５は、多関節型（６軸）ロボット１を示す（ａ）平面図、（ｂ）側面図である。このロボット１は、ベース２上に６軸のアームＲＡを有し、そのアームＲＡの先端にハンド９が取り付けられている。即ち、前記ベース２上には、第１関節Ｊ１を介して第１のアーム３が回転可能に連結されている。この第１のアーム３には、第２関節Ｊ２を介して第２のアーム４の下端部が回転可能に連結され、さらに、この第２のアーム４の先端部には、第３関節Ｊ３を介して第３のアーム５が回転可能に連結されている。
【０００７】
この第３のアーム５の先端には第４関節Ｊ４を介して第４のアーム６が回転可能に連結され、この第４のアーム６の先端には第５関節Ｊ５を介して第５のアーム７が回転可能に連結され、この第５のアーム７には第６関節Ｊ６を介して第６のアーム８が回転可能に連結されている。なお、各関節Ｊ１〜Ｊ６は、例えば夫々に配置されるサーボモータ２２（図６参照）により各アーム３〜８を回転駆動するようになっている。また、図４には、アームＲＡの各軸構成をモデル的に示す。
そして、アーム８には、ハンド９が取り付けられている。この場合、関節Ｊ２，Ｊ３の角度θ２，θ３の和が１８０度であれば、関節Ｊ４，Ｊ５，Ｊ６の回動動作は、夫々ハンド９のヨー，ピッチ，ロールに対応することになる。また、ベース２は、レールに沿って移動可能となっており、そのための駆動機構（図示せず）も内蔵している。尚、レールはロボット１の走行軸（冗長軸）となっている。
【０００８】
図６は、ロボット１及びその制御系の構成を概略的に示す機能ブロック図である。教示ペンダント１１は操作インターフェイス（Ｉ／Ｆ）であり、操作者は、ハンド９を移動させたい方向に教示ペンダント１１の操作キーを操作する。すると、その操作キーの押し度合いに応じた信号（ＯＮ／ＯＦＦ）を、マン・マシン・インターフェイスとしての例えば目標指令生成部１３に内蔵される信号入力ボード１４に与える。
【０００９】
信号入力ボード１４は、操作信号１２により与えられる信号をＣＰＵ１５に出力すると、ＣＰＵ１５は、その操作信号に応じてロボット１のアーム先端（即ち、ハンド９）部分を移動させるための目標速度［ｍ／ｓｅｃ］を換算する。そして、その目標速度を、例えばイーサネット（登録商標）などに対応した通信デバイス１６を介してロボット制御用のコントローラ（制御装置）１８に送信する。コントローラ１８のＣＰＵ１９は、制御プログラム（コンピュータプログラム）２４に従って処理を実行するもので、受信した目標速度に基づいてロボット１の走行軸上の移動量を算出し、また、アームＲＡの目標位置や姿勢角に関連する目標指令値などを算出する。そして、ＣＰＵ１９は、アーム各軸の指令値［ｍ］を生成すると、制御ボード２０を介してロボット１側のモータドライバ２１に出力する。
モータドライバ２１は、与えられた指令値に応じてアームＲＡの各軸に配置されているモータ２２を駆動する。すると、モータ２２による軸変位量はエンコーダ２３により検出されて、変位量に応じたパルス信号がモータドライバ２１に出力される。
【００１０】
次に、本実施例の作用について図１乃至図３も参照して説明する。図１はコントローラ１８のＣＰＵ１９によって実行されるロボット１の各軸指令値を求める処理の内容を示すフローチャートである。また、図２は、図１の処理に応じて先端位置が移動されるロボット１のアームＲＡの（ベクトル）モデルである。ＣＰＵ１９は、上述したように目標指令生成部１３側より送信された目標速度を取得すると（ステップＳ１）、その目標速度に基づいて、ロボット１のアームＲＡ先端をサンプリング時間ｄｔ内に移動させる目標移動量（ΔＸ，ΔＹ，ΔＺ）を求める（ステップＳ２）。
【００１１】
続いて、ＣＰＵ１９は、アームＲＡ先端の現在位置ＡＰ０（Ｘ０，Ｙ０，Ｚ０）を取得すると（ステップＳ３）、走行軸上の移動を行わない状態で（即ち、原点ΣＡの座標系において）、現在位置（Ｘ０，Ｙ０，Ｚ０）及び目標移動量（ΔＸ，ΔＹ，ΔＺ）からアームＲＡ先端の目標位置ＡＰ１（Ｘｃ，Ｙｃ，Ｚｃ）を算出する（ステップＳ４，図２（ａ））。尚、目標位置（Ｘｃ，Ｙｃ，Ｚｃ）は、アームＲＡ先端における姿勢角（ヨー，ピッチ，ロール）の変位がある場合は、同次変換行列の積によって演算される。
【００１２】
次に、ＣＰＵ１９は、アームＲＡの先端を目標位置（Ｘｃ，Ｙｃ，Ｚｃ）に移動させるための走行軸上の移動量ΔＸ３を算出すると（ステップＳ５，図２（ｂ））、その移動量ΔＸ３だけベース２を移動させた後の新たな座標系（原点ΣＢ）で表現される目標位置ＢＰ１（Ｘ１，Ｙ１，Ｚ１）を算出する（ステップＳ６，図２（ｃ））。そして、目標位置ＢＰ１（Ｘ１，Ｙ１，Ｚ１），及び姿勢角の目標値（Ｒｘ１，Ｒｙ１，Ｒｚ１）に基づきアームＲＡ各軸の目標位置、移動速度を算出すると（ステップＳ７）、それらに応じた各軸毎の指令値を出力する（ステップＳ８）。それから、ステップＳ１に戻る。即ち、以上の処理は走行軸協調制御である。
【００１３】
次に、ステップＳ５における走行軸上の移動量ΔＸ３の算出について、図３を参照して説明する。図３（ａ）はアームＲＡのｘｚ座標系（垂直面）であり、（ｂ）は同ｘｙ座標系（水平面）である。図３（ａ）において、座標原点ΣＢから関節Ｊ２までの高さをｄ２，関節Ｊ２がｚ軸に対してなす角度をθ２，関節Ｊ２から関節Ｊ３までの距離（＝アーム４）をＬ２，関節Ｊ３がアーム４に対してなす角度をθ３，関節Ｊ３から関節Ｊ５までの距離（＝アーム５）をＬ３としている。尚、図５に示すようにハンド９はアーム５の先端、即ち関節Ｊ５の位置に取り付けられているので、関節Ｊ５がアームＲＡの先端となる。
【００１４】
また、図３（ｂ）において、関節Ｊ１がｘ軸に対してなす角度をθ２としている。この場合、ｘ軸が走行軸に一致している。そして、図３は、原点ΣＡから走行軸上をΔＸ３移動させた原点ΣＢの座標系であり、従って、アームＲＡの先端は目標位置（Ｘｃ，Ｙｃ，Ｚｃ）を示している。
すると、目標位置（Ｘｃ，Ｙｃ，Ｚｃ）の各座標は、原点ΣＡを基準として以下のように表される。
Ｘｃ＝｛Ｌ２・ｓｉｎθ２＋Ｌ３・ｓｉｎ（θ２＋θ３）｝ｃｏｓθ１＋ΔＸ３
・・・（１）
Ｙｃ＝｛Ｌ２・ｓｉｎθ２＋Ｌ３・ｓｉｎ（θ２＋θ３）｝ｓｉｎθ１・・・（２）
Ｚｃ＝ Ｌ２・ｃｏｓθ２＋ｄ２−Ｌ３・ｃｏｓ（π−θ２−θ３）
＝ Ｌ２・ｃｏｓθ２＋ｄ２＋Ｌ３・ｃｏｓ（θ２＋θ３） ・・・（３）
ここで、移動量ΔＸ３は、サンプリング時間毎にベース２が移動する走行軸上の距離であり、目標位置（Ｘｃ，Ｙｃ，Ｚｃ）が決まると、ｄ２，Ｌ２，Ｌ３，θ１〜θ３の６つの自由度を設定することで（１）〜（３）式に基づいて移動量ΔＸ３を求めることになる。尚、ｄ２，Ｌ２，Ｌ３は固定値である。また、（θ２＋θ３）は、アームＲＡの先端に取り付けられたハンド９の姿勢制御軸であるＪ４〜Ｊ６の基準位置を一定に保つため、不変とすることを拘束条件とする。
【００１５】
（１）式より、
Ｘｃ−ΔＸ３＝｛Ｌ２・ｓｉｎθ２＋Ｌ３・ｓｉｎ（θ２＋θ３）｝ｃｏｓθ１
・・・（４）
ここで、α＝Ｌ２・ｓｉｎθ２＋Ｌ３・ｓｉｎ（θ２＋θ３） とすると、
Ｘｃ−ΔＸ３＝α・ｃｏｓθ１ ・・・（５）
Ｙｃ＝α・ｓｉｎθ１ ・・・（６）
よって、
（Ｘｃ−ΔＸ３）2＋Ｙｃ2＝α2 ・・・（７）
また、（３）式より、
Ｌ２・ｃｏｓθ２＝Ｚｃ−ｄ２−Ｌ３・ｃｏｓ（θ２＋θ３） ・・・（８）
（７）式より、
Ｌ２・ｓｉｎθ２＋Ｌ３・ｓｉｎ（θ２＋θ３）
＝±｛（Ｘｃ−ΔＸ３）2＋Ｙｃ2｝1/2
Ｌ２・ｓｉｎθ２＝−Ｌ３・ｓｉｎ（θ２＋θ３）
±｛（Ｘｃ−ΔＸ３）2＋Ｙｃ2｝1/2 ・・・（９）
（８）、（９）式より、
Ｌ２2（ｃｏｓ2θ２＋ｓｉｎ2θ２）
＝｛Ｚｃ−ｄ２−Ｌ３・ｃｏｓ（θ２＋θ３）｝2
＋［−Ｌ３・ｓｉｎ（θ２＋θ３）±｛（Ｘｃ−ΔＸ３）2＋Ｙｃ2｝1/2］2
ここで、β＝Ｚｃ−ｄ２−Ｌ３・ｃｏｓ（θ２＋θ３）
γ＝Ｌ３・ｓｉｎ（θ２＋θ３） とすると、上記拘束条件により
（θ２＋θ３）＝ｃｏｎｓｔ．であるから、β及びγは固定値となる。
【００１６】
Ｌ２2＝β2＋｛−γ±｛（Ｘｃ−ΔＸ３）2＋Ｙｃ2｝1/2｝2
−γ±｛（Ｘｃ−ΔＸ３）2＋Ｙｃ2｝1/2＝±（Ｌ２2−β2）1/2
±｛（Ｘｃ−ΔＸ３）2＋Ｙｃ2｝1/2＝γ±（Ｌ２2−β2）1/2
（Ｘｃ−ΔＸ３）2＋Ｙｃ2＝｛γ±（Ｌ２2−β2）1/2｝2
ΔＸ３＝Ｘｃ±［｛γ±（Ｌ２2−β2）1/2｝2−Ｙｃ2］1/2 ・・・（１０）
以上のようにΔＸ３が求められる。ここで、符号の正負の組み合わせにより（１０）式からは以下の４つの解が得られる。
ΔＸ３１＝Ｘｃ＋［｛γ＋（Ｌ２2−β2）1/2｝2−Ｙｃ2］1/2
ΔＸ３２＝Ｘｃ＋［｛γ−（Ｌ２2−β2）1/2｝2−Ｙｃ2］1/2
ΔＸ３３＝Ｘｃ−［｛γ＋（Ｌ２2−β2）1/2｝2−Ｙｃ2］1/2
ΔＸ３４＝Ｘｃ−［｛γ−（Ｌ２2−β2）1/2｝2−Ｙｃ2］1/2
これら４つの内、絶対値が最小となるものをΔＸ３として選択する。
【００１７】
以上のように本実施例によれば、ロボット１の制御を行うコントローラ１８は、垂直多関節型で６軸のアームＲＡの先端について、現在位置（Ｘ０，Ｙ０，Ｚ０）と所定時間後の目標移動量（ΔＸ，ΔＹ，ΔＺ）とに基づいて決定される目標位置（Ｘｃ，Ｙｃ，Ｚｃ）に対し、その時点でアームＲＡが保持している第２関節角度θ２と第３関節角度θ３との和が一定となることを拘束条件として、アームＲＡの先端を目標位置に移動させるために必要な走行軸上における所定時間当たりの移動量ΔＸ３を計算するようにした。
従って、走行軸上の移動量ΔＸ３については指定を行う必要がなくなり、アームＲＡの先端を目標位置に到達させるためのロボット制御、若しくはそのためのティーチングをより簡単に行うことができる。
【００１８】
（第２実施例）
図８は本発明の第２実施例を示すものであり、第１実施例と同一部分には同一符号を付して説明を省略し、以下異なる部分についてのみ説明する。第２実施例の構成は基本的に第１実施例と同様であり、コントローラ１８のＣＰＵ１９によって実行される制御内容が第１実施例と若干異なっている。
即ち、図８に示すように、ＣＰＵ１９は、ステップＳ７において、各軸の目標位置及び目標速度を算出すると、走行軸に関する目標速度ΔＶ３（＝ΔＸ３／ｄｔ：ｄｔは、サンプリング時間）を上限速度Ｖmaxと比較してΔＶ３＞Ｖmaxか否かを判断する（ステップＳ１１）。そして、ΔＶ３≦Ｖmaxであれば（「ＮＯ」）そのままステップＳ８に移行する。
【００１９】
一方、ステップＳ１１においてΔＶ３＞Ｖmaxであれば（「ＹＥＳ」）、ＣＰＵ１９は両者の差ｄＶ３（＝Ｖmax−ΔＶ３）を演算する（ステップＳ１２）。そして、求めた差ｄＶ３に、サンプリング時間ｄｔをかけた距離をｘ座標値Ｘ１に加える（ステップＳ１３）。但し、この場合、ステップＳ１２で求めた差ｄＶ３の符号は負であるから、実質的には、Ｘ１−｜ｄＶ３｜・ｄｔである。それから、ステップＳ８に移行する。この場合、走行軸上の移動速度は、上限速度Ｖmaxで規制されることになる。
【００２０】
即ち、拘束条件（θ２＋θ３＝一定）を与えて求めた目標移動量ΔＸ３だけ、アームＲＡの先端を所定時間ｄｔ内に移動させようとすると、ロボット１の動作条件として設定されている上限速度Ｖmaxを超えてしまう場合が想定される。そして、上限速度Ｖmaxを超える速度でアームＲＡを動作させると、ロボット１の機構に過大な負担をかけてしまうおそれがある。従って、そのようなケースでは、上限速度Ｖmaxを超えない範囲で移動させることを優先して、上記のように拘束条件を解除して走行軸上の移動距離を短縮するように修正する。
【００２１】
以上のように第２実施例によれば、ＣＰＵ１９は、走行軸上における所定時間ｄｔ当たりの移動量ΔＸ３に基づいて求めた移動速度ΔＶ３が上限速度Ｖmaxを超える場合には、拘束条件を解除して移動速度ΔＶ３を上限速度Ｖmaxに置き換え、両速度の差ｄＶ３に応じた移動量を走行軸上の座標値Ｘ１より減じて目標位置を修正するので、ロボット１の機構に過大な負担をかける事態を回避することができる。
【００２２】
本発明は上記し又は図面に記載した実施例にのみ限定されるものではなく、次のような変形が可能である。
走行軸上の移動量について作業者により直接入力が行われた場合は、本発明の手法を用いることなく、前記入力操作量に応じてロボット１をレール上で移動させれば良い。
オンライン，オフラインティーチングの場合も、同様に適用が可能である。
ハンドリング作業用に限らず、例えば組立てや塗装，溶接などの広範囲での作業を必要とするため、走行軸により可動範囲を広げるようにしたものにも適用することができる。また、生産設備のセル内で稼動するロボットにおいて、アーム長を抑えつつ可動範囲を広げるため走行軸を用いるものに適用することができる。
【図面の簡単な説明】
【００２３】
【図１】本発明をハンドリング作業用ロボットに適用した場合の第１実施例であり、コントローラのＣＰＵにより実行される、ロボットの各軸指令値を求める処理の内容を示すフローチャート
【図２】図１の処理に応じて先端位置が移動されるロボットアームのベクトルモデル図
【図３】（ａ）はアームのｘｚ座標系（垂直面）、（ｂ）は同ｘｙ座標系（水平面）を示す図
【図４】アームの各軸構成をモデル的に示す図
【図５】ロボットの（ａ）平面図、（ｂ）側面図
【図６】ロボット及びその制御系の構成を概略的に示す機能ブロック図
【図７】本発明の第２実施例を示す図１相当図
【符号の説明】
【００２４】
図面中、１はロボット、９はハンド、１８はコントローラ（制御装置）、１９はＣＰＵ、２４は制御プログラム（コンピュータプログラム）、ＲＡはアームを示す。
【技術分野】
【０００１】
本発明は、走行軸を冗長軸とし、垂直多関節型で６軸のアームを有するロボットの制御を行う制御装置及び制御方法、並びに前記制御装置を構成するコンピュータにより実行されるプログラムに関する。
【背景技術】
【０００２】
走行軸を冗長軸として、垂直多関節型で６軸のアームを有するロボットには、計７つの自由度が存在する。従って、上記ロボットを制御してアームの先端を目標位置まで移動させるには、それら７つの自由度を全て規定する必要があり、ティーチング作業やリアルタイム操作が煩雑であるという問題がある。
例えば、特許文献１には、上記と同様に構成されるロボットについて、冗長軸に関するティーチング時間を「０」とするようにしたプログラムの作成方法及び装置が開示されている。この技術は、冗長軸が取り得る位置を予め複数の離散的な位置に定めておき、オフラインティーチング時に得られるティーチングデータから、ティーチング時における複数の動作状態でのアーム先端部と冗長軸である走行台車の原点位置との距離を演算し、各演算結果に基づいて、動作プログラム中における走行台車の位置を予め定めた離散的な位置の何れかに自動的に決定する、というものである。
【特許文献１】特開平１０−２４４４８１号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、特許文献１の技術では、事前に冗長軸が取り得る位置を定める必要があり、しかも、実際の動作時におけるロボットの冗長軸上の位置は定められた何れかに限定されてしまう。従って、ロボットの制御を柔軟に行うことができないという問題がある。
本発明は上記事情に鑑みてなされたものであり、その目的は、ロボットが使用される状況に応じて要求される条件を考慮することで、アーム先端を目標位置に移動させるために必要な走行軸の移動量を自動的に決定することができるロボット制御装置及び制御方法、並びに前記制御装置を構成するコンピュータにより実行されるプログラムを提供することにある。
【課題を解決するための手段】
【０００４】
請求項１記載のロボット制御装置によれば、垂直多関節型で６軸のアームの先端について、現在位置と所定時間後の目標移動量とに基づいて決定される目標位置に対し、その時点でアームが保持している第２関節角度θ２と第３関節角度θ３との和が一定となることを拘束条件として、アームの先端を目標位置に移動させるために必要な走行軸上における所定時間当たりの移動量を計算する。
即ち、垂直多関節型で６軸のアームの先端を目標位置に移動させる場合には、現在位置として、アームの第１〜第３関節角度θ１〜θ３と、アームの原点から第２関節までの高さｄ２と、第２関節と第３関節との距離Ｌ２と、第３関節と前記アームの先端までの距離Ｌ３とがパラメータとして与えられる。また、第４〜第６関節の角度θ４〜θ６は、これらのパラメータと連動して、アーム先端に取り付けられている若しくはアーム先端のハンド等によって把持されている物体の姿勢を決定することになる。そして、上記のパラメータに加えて、アーム原点の走行軸上の移動量を決定することでアームの先端を目標位置に到達させる。
【０００５】
請求項２記載のロボット制御装置によれば、走行軸上における所定時間当たりの移動量に基づいて求めた移動速度が上限速度を超える場合には、拘束条件を解除して前記移動速度を上限速度に置き換える。そして、両速度の差に応じた移動量を走行軸上の座標値より減じて目標位置を修正する。即ち、請求項１のように設定した拘束条件に基づいて求めた移動量だけ、アームの先端を所定時間内に移動させようとすると、ロボットの動作条件として設定されている上限速度を超えてしまう場合が想定される。従って、上記のように修正を行えば、そのような事態の発生を回避することができる。
【発明を実施するための最良の形態】
【０００６】
（第１実施例）
以下、本発明をハンドリング作業用ロボットに適用した場合の第１実施例について図１乃至図６を参照して説明する。図５は、多関節型（６軸）ロボット１を示す（ａ）平面図、（ｂ）側面図である。このロボット１は、ベース２上に６軸のアームＲＡを有し、そのアームＲＡの先端にハンド９が取り付けられている。即ち、前記ベース２上には、第１関節Ｊ１を介して第１のアーム３が回転可能に連結されている。この第１のアーム３には、第２関節Ｊ２を介して第２のアーム４の下端部が回転可能に連結され、さらに、この第２のアーム４の先端部には、第３関節Ｊ３を介して第３のアーム５が回転可能に連結されている。
【０００７】
この第３のアーム５の先端には第４関節Ｊ４を介して第４のアーム６が回転可能に連結され、この第４のアーム６の先端には第５関節Ｊ５を介して第５のアーム７が回転可能に連結され、この第５のアーム７には第６関節Ｊ６を介して第６のアーム８が回転可能に連結されている。なお、各関節Ｊ１〜Ｊ６は、例えば夫々に配置されるサーボモータ２２（図６参照）により各アーム３〜８を回転駆動するようになっている。また、図４には、アームＲＡの各軸構成をモデル的に示す。
そして、アーム８には、ハンド９が取り付けられている。この場合、関節Ｊ２，Ｊ３の角度θ２，θ３の和が１８０度であれば、関節Ｊ４，Ｊ５，Ｊ６の回動動作は、夫々ハンド９のヨー，ピッチ，ロールに対応することになる。また、ベース２は、レールに沿って移動可能となっており、そのための駆動機構（図示せず）も内蔵している。尚、レールはロボット１の走行軸（冗長軸）となっている。
【０００８】
図６は、ロボット１及びその制御系の構成を概略的に示す機能ブロック図である。教示ペンダント１１は操作インターフェイス（Ｉ／Ｆ）であり、操作者は、ハンド９を移動させたい方向に教示ペンダント１１の操作キーを操作する。すると、その操作キーの押し度合いに応じた信号（ＯＮ／ＯＦＦ）を、マン・マシン・インターフェイスとしての例えば目標指令生成部１３に内蔵される信号入力ボード１４に与える。
【０００９】
信号入力ボード１４は、操作信号１２により与えられる信号をＣＰＵ１５に出力すると、ＣＰＵ１５は、その操作信号に応じてロボット１のアーム先端（即ち、ハンド９）部分を移動させるための目標速度［ｍ／ｓｅｃ］を換算する。そして、その目標速度を、例えばイーサネット（登録商標）などに対応した通信デバイス１６を介してロボット制御用のコントローラ（制御装置）１８に送信する。コントローラ１８のＣＰＵ１９は、制御プログラム（コンピュータプログラム）２４に従って処理を実行するもので、受信した目標速度に基づいてロボット１の走行軸上の移動量を算出し、また、アームＲＡの目標位置や姿勢角に関連する目標指令値などを算出する。そして、ＣＰＵ１９は、アーム各軸の指令値［ｍ］を生成すると、制御ボード２０を介してロボット１側のモータドライバ２１に出力する。
モータドライバ２１は、与えられた指令値に応じてアームＲＡの各軸に配置されているモータ２２を駆動する。すると、モータ２２による軸変位量はエンコーダ２３により検出されて、変位量に応じたパルス信号がモータドライバ２１に出力される。
【００１０】
次に、本実施例の作用について図１乃至図３も参照して説明する。図１はコントローラ１８のＣＰＵ１９によって実行されるロボット１の各軸指令値を求める処理の内容を示すフローチャートである。また、図２は、図１の処理に応じて先端位置が移動されるロボット１のアームＲＡの（ベクトル）モデルである。ＣＰＵ１９は、上述したように目標指令生成部１３側より送信された目標速度を取得すると（ステップＳ１）、その目標速度に基づいて、ロボット１のアームＲＡ先端をサンプリング時間ｄｔ内に移動させる目標移動量（ΔＸ，ΔＹ，ΔＺ）を求める（ステップＳ２）。
【００１１】
続いて、ＣＰＵ１９は、アームＲＡ先端の現在位置ＡＰ０（Ｘ０，Ｙ０，Ｚ０）を取得すると（ステップＳ３）、走行軸上の移動を行わない状態で（即ち、原点ΣＡの座標系において）、現在位置（Ｘ０，Ｙ０，Ｚ０）及び目標移動量（ΔＸ，ΔＹ，ΔＺ）からアームＲＡ先端の目標位置ＡＰ１（Ｘｃ，Ｙｃ，Ｚｃ）を算出する（ステップＳ４，図２（ａ））。尚、目標位置（Ｘｃ，Ｙｃ，Ｚｃ）は、アームＲＡ先端における姿勢角（ヨー，ピッチ，ロール）の変位がある場合は、同次変換行列の積によって演算される。
【００１２】
次に、ＣＰＵ１９は、アームＲＡの先端を目標位置（Ｘｃ，Ｙｃ，Ｚｃ）に移動させるための走行軸上の移動量ΔＸ３を算出すると（ステップＳ５，図２（ｂ））、その移動量ΔＸ３だけベース２を移動させた後の新たな座標系（原点ΣＢ）で表現される目標位置ＢＰ１（Ｘ１，Ｙ１，Ｚ１）を算出する（ステップＳ６，図２（ｃ））。そして、目標位置ＢＰ１（Ｘ１，Ｙ１，Ｚ１），及び姿勢角の目標値（Ｒｘ１，Ｒｙ１，Ｒｚ１）に基づきアームＲＡ各軸の目標位置、移動速度を算出すると（ステップＳ７）、それらに応じた各軸毎の指令値を出力する（ステップＳ８）。それから、ステップＳ１に戻る。即ち、以上の処理は走行軸協調制御である。
【００１３】
次に、ステップＳ５における走行軸上の移動量ΔＸ３の算出について、図３を参照して説明する。図３（ａ）はアームＲＡのｘｚ座標系（垂直面）であり、（ｂ）は同ｘｙ座標系（水平面）である。図３（ａ）において、座標原点ΣＢから関節Ｊ２までの高さをｄ２，関節Ｊ２がｚ軸に対してなす角度をθ２，関節Ｊ２から関節Ｊ３までの距離（＝アーム４）をＬ２，関節Ｊ３がアーム４に対してなす角度をθ３，関節Ｊ３から関節Ｊ５までの距離（＝アーム５）をＬ３としている。尚、図５に示すようにハンド９はアーム５の先端、即ち関節Ｊ５の位置に取り付けられているので、関節Ｊ５がアームＲＡの先端となる。
【００１４】
また、図３（ｂ）において、関節Ｊ１がｘ軸に対してなす角度をθ２としている。この場合、ｘ軸が走行軸に一致している。そして、図３は、原点ΣＡから走行軸上をΔＸ３移動させた原点ΣＢの座標系であり、従って、アームＲＡの先端は目標位置（Ｘｃ，Ｙｃ，Ｚｃ）を示している。
すると、目標位置（Ｘｃ，Ｙｃ，Ｚｃ）の各座標は、原点ΣＡを基準として以下のように表される。
Ｘｃ＝｛Ｌ２・ｓｉｎθ２＋Ｌ３・ｓｉｎ（θ２＋θ３）｝ｃｏｓθ１＋ΔＸ３
・・・（１）
Ｙｃ＝｛Ｌ２・ｓｉｎθ２＋Ｌ３・ｓｉｎ（θ２＋θ３）｝ｓｉｎθ１・・・（２）
Ｚｃ＝ Ｌ２・ｃｏｓθ２＋ｄ２−Ｌ３・ｃｏｓ（π−θ２−θ３）
＝ Ｌ２・ｃｏｓθ２＋ｄ２＋Ｌ３・ｃｏｓ（θ２＋θ３） ・・・（３）
ここで、移動量ΔＸ３は、サンプリング時間毎にベース２が移動する走行軸上の距離であり、目標位置（Ｘｃ，Ｙｃ，Ｚｃ）が決まると、ｄ２，Ｌ２，Ｌ３，θ１〜θ３の６つの自由度を設定することで（１）〜（３）式に基づいて移動量ΔＸ３を求めることになる。尚、ｄ２，Ｌ２，Ｌ３は固定値である。また、（θ２＋θ３）は、アームＲＡの先端に取り付けられたハンド９の姿勢制御軸であるＪ４〜Ｊ６の基準位置を一定に保つため、不変とすることを拘束条件とする。
【００１５】
（１）式より、
Ｘｃ−ΔＸ３＝｛Ｌ２・ｓｉｎθ２＋Ｌ３・ｓｉｎ（θ２＋θ３）｝ｃｏｓθ１
・・・（４）
ここで、α＝Ｌ２・ｓｉｎθ２＋Ｌ３・ｓｉｎ（θ２＋θ３） とすると、
Ｘｃ−ΔＸ３＝α・ｃｏｓθ１ ・・・（５）
Ｙｃ＝α・ｓｉｎθ１ ・・・（６）
よって、
（Ｘｃ−ΔＸ３）2＋Ｙｃ2＝α2 ・・・（７）
また、（３）式より、
Ｌ２・ｃｏｓθ２＝Ｚｃ−ｄ２−Ｌ３・ｃｏｓ（θ２＋θ３） ・・・（８）
（７）式より、
Ｌ２・ｓｉｎθ２＋Ｌ３・ｓｉｎ（θ２＋θ３）
＝±｛（Ｘｃ−ΔＸ３）2＋Ｙｃ2｝1/2
Ｌ２・ｓｉｎθ２＝−Ｌ３・ｓｉｎ（θ２＋θ３）
±｛（Ｘｃ−ΔＸ３）2＋Ｙｃ2｝1/2 ・・・（９）
（８）、（９）式より、
Ｌ２2（ｃｏｓ2θ２＋ｓｉｎ2θ２）
＝｛Ｚｃ−ｄ２−Ｌ３・ｃｏｓ（θ２＋θ３）｝2
＋［−Ｌ３・ｓｉｎ（θ２＋θ３）±｛（Ｘｃ−ΔＸ３）2＋Ｙｃ2｝1/2］2
ここで、β＝Ｚｃ−ｄ２−Ｌ３・ｃｏｓ（θ２＋θ３）
γ＝Ｌ３・ｓｉｎ（θ２＋θ３） とすると、上記拘束条件により
（θ２＋θ３）＝ｃｏｎｓｔ．であるから、β及びγは固定値となる。
【００１６】
Ｌ２2＝β2＋｛−γ±｛（Ｘｃ−ΔＸ３）2＋Ｙｃ2｝1/2｝2
−γ±｛（Ｘｃ−ΔＸ３）2＋Ｙｃ2｝1/2＝±（Ｌ２2−β2）1/2
±｛（Ｘｃ−ΔＸ３）2＋Ｙｃ2｝1/2＝γ±（Ｌ２2−β2）1/2
（Ｘｃ−ΔＸ３）2＋Ｙｃ2＝｛γ±（Ｌ２2−β2）1/2｝2
ΔＸ３＝Ｘｃ±［｛γ±（Ｌ２2−β2）1/2｝2−Ｙｃ2］1/2 ・・・（１０）
以上のようにΔＸ３が求められる。ここで、符号の正負の組み合わせにより（１０）式からは以下の４つの解が得られる。
ΔＸ３１＝Ｘｃ＋［｛γ＋（Ｌ２2−β2）1/2｝2−Ｙｃ2］1/2
ΔＸ３２＝Ｘｃ＋［｛γ−（Ｌ２2−β2）1/2｝2−Ｙｃ2］1/2
ΔＸ３３＝Ｘｃ−［｛γ＋（Ｌ２2−β2）1/2｝2−Ｙｃ2］1/2
ΔＸ３４＝Ｘｃ−［｛γ−（Ｌ２2−β2）1/2｝2−Ｙｃ2］1/2
これら４つの内、絶対値が最小となるものをΔＸ３として選択する。
【００１７】
以上のように本実施例によれば、ロボット１の制御を行うコントローラ１８は、垂直多関節型で６軸のアームＲＡの先端について、現在位置（Ｘ０，Ｙ０，Ｚ０）と所定時間後の目標移動量（ΔＸ，ΔＹ，ΔＺ）とに基づいて決定される目標位置（Ｘｃ，Ｙｃ，Ｚｃ）に対し、その時点でアームＲＡが保持している第２関節角度θ２と第３関節角度θ３との和が一定となることを拘束条件として、アームＲＡの先端を目標位置に移動させるために必要な走行軸上における所定時間当たりの移動量ΔＸ３を計算するようにした。
従って、走行軸上の移動量ΔＸ３については指定を行う必要がなくなり、アームＲＡの先端を目標位置に到達させるためのロボット制御、若しくはそのためのティーチングをより簡単に行うことができる。
【００１８】
（第２実施例）
図８は本発明の第２実施例を示すものであり、第１実施例と同一部分には同一符号を付して説明を省略し、以下異なる部分についてのみ説明する。第２実施例の構成は基本的に第１実施例と同様であり、コントローラ１８のＣＰＵ１９によって実行される制御内容が第１実施例と若干異なっている。
即ち、図８に示すように、ＣＰＵ１９は、ステップＳ７において、各軸の目標位置及び目標速度を算出すると、走行軸に関する目標速度ΔＶ３（＝ΔＸ３／ｄｔ：ｄｔは、サンプリング時間）を上限速度Ｖmaxと比較してΔＶ３＞Ｖmaxか否かを判断する（ステップＳ１１）。そして、ΔＶ３≦Ｖmaxであれば（「ＮＯ」）そのままステップＳ８に移行する。
【００１９】
一方、ステップＳ１１においてΔＶ３＞Ｖmaxであれば（「ＹＥＳ」）、ＣＰＵ１９は両者の差ｄＶ３（＝Ｖmax−ΔＶ３）を演算する（ステップＳ１２）。そして、求めた差ｄＶ３に、サンプリング時間ｄｔをかけた距離をｘ座標値Ｘ１に加える（ステップＳ１３）。但し、この場合、ステップＳ１２で求めた差ｄＶ３の符号は負であるから、実質的には、Ｘ１−｜ｄＶ３｜・ｄｔである。それから、ステップＳ８に移行する。この場合、走行軸上の移動速度は、上限速度Ｖmaxで規制されることになる。
【００２０】
即ち、拘束条件（θ２＋θ３＝一定）を与えて求めた目標移動量ΔＸ３だけ、アームＲＡの先端を所定時間ｄｔ内に移動させようとすると、ロボット１の動作条件として設定されている上限速度Ｖmaxを超えてしまう場合が想定される。そして、上限速度Ｖmaxを超える速度でアームＲＡを動作させると、ロボット１の機構に過大な負担をかけてしまうおそれがある。従って、そのようなケースでは、上限速度Ｖmaxを超えない範囲で移動させることを優先して、上記のように拘束条件を解除して走行軸上の移動距離を短縮するように修正する。
【００２１】
以上のように第２実施例によれば、ＣＰＵ１９は、走行軸上における所定時間ｄｔ当たりの移動量ΔＸ３に基づいて求めた移動速度ΔＶ３が上限速度Ｖmaxを超える場合には、拘束条件を解除して移動速度ΔＶ３を上限速度Ｖmaxに置き換え、両速度の差ｄＶ３に応じた移動量を走行軸上の座標値Ｘ１より減じて目標位置を修正するので、ロボット１の機構に過大な負担をかける事態を回避することができる。
【００２２】
本発明は上記し又は図面に記載した実施例にのみ限定されるものではなく、次のような変形が可能である。
走行軸上の移動量について作業者により直接入力が行われた場合は、本発明の手法を用いることなく、前記入力操作量に応じてロボット１をレール上で移動させれば良い。
オンライン，オフラインティーチングの場合も、同様に適用が可能である。
ハンドリング作業用に限らず、例えば組立てや塗装，溶接などの広範囲での作業を必要とするため、走行軸により可動範囲を広げるようにしたものにも適用することができる。また、生産設備のセル内で稼動するロボットにおいて、アーム長を抑えつつ可動範囲を広げるため走行軸を用いるものに適用することができる。
【図面の簡単な説明】
【００２３】
【図１】本発明をハンドリング作業用ロボットに適用した場合の第１実施例であり、コントローラのＣＰＵにより実行される、ロボットの各軸指令値を求める処理の内容を示すフローチャート
【図２】図１の処理に応じて先端位置が移動されるロボットアームのベクトルモデル図
【図３】（ａ）はアームのｘｚ座標系（垂直面）、（ｂ）は同ｘｙ座標系（水平面）を示す図
【図４】アームの各軸構成をモデル的に示す図
【図５】ロボットの（ａ）平面図、（ｂ）側面図
【図６】ロボット及びその制御系の構成を概略的に示す機能ブロック図
【図７】本発明の第２実施例を示す図１相当図
【符号の説明】
【００２４】
図面中、１はロボット、９はハンド、１８はコントローラ（制御装置）、１９はＣＰＵ、２４は制御プログラム（コンピュータプログラム）、ＲＡはアームを示す。
【特許請求の範囲】
【請求項１】
走行軸を冗長軸とし、垂直多関節型で６軸のアームを有するロボットの制御を行う制御装置において、
前記アームの先端について、現在位置と、所定時間後の目標移動量とに基づいて決定される目標位置に対し、
その時点で前記アームが保持している第２関節角度θ２と第３関節角度θ３との和が一定となることを拘束条件として、前記アームの先端を前記目標位置に移動させるために必要な、前記走行軸上における前記所定時間当たりの移動量を計算することを特徴とするロボット制御装置。
【請求項２】
前記走行軸上における前記所定時間当たりの移動量に基づいて移動速度を求め、前記移動速度が上限速度を超える場合には、上記拘束条件を解除して前記移動速度を上限速度に置き換えると共に、両速度の差に応じた移動量を前記走行軸上の座標値より減じて前記目標位置を修正することを特徴とする請求項１記載のロボット制御装置。
【請求項３】
走行軸を冗長軸とし、垂直多関節型で６軸のアームを有するロボットの制御を行う制御方法において、
前記アームの先端について、現在位置と、所定時間後の目標移動量とに基づいて決定される目標位置に対し、
その時点で前記アームが保持している第２関節角度θ２と第３関節角度θ３との和が一定となることを拘束条件として、前記アームの先端を前記目標位置に移動させるために必要な、前記走行軸上における前記所定時間当たりの移動量を計算することを特徴とするロボット制御方法。
【請求項４】
前記走行軸上における前記所定時間当たりの移動量に基づいて移動速度を求め、
前記移動速度が上限速度を超える場合には、上記拘束条件を解除して前記移動速度を上限速度に置き換え、
両速度の差に応じた移動量を前記走行軸上の座標値より減じて前記目標位置を修正することを特徴とする請求項３記載のロボット制御方法。
【請求項５】
走行軸を冗長軸とし、垂直多関節型で６軸のアームを有するロボットの制御を行う制御装置を構成するコンピュータにより実行されるプログラムにおいて、
前記アームの先端について、現在位置と、所定時間後の目標移動量とに基づいて決定される目標位置に対し、
その時点で前記アームが保持している第２関節角度θ２と第３関節角度θ３との和が一定となることを拘束条件として、前記アームの先端を前記目標位置に移動させるために必要な、前記走行軸上における前記所定時間当たりの移動量を計算させることを特徴とするコンピュータプログラム。
【請求項６】
前記走行軸上における前記所定時間当たりの移動量に基づいて移動速度を求めさせ、
前記移動速度が上限速度を超える場合には、上記拘束条件を解除して前記移動速度を上限速度に置き換えさせ、
両速度の差に応じた移動量を前記走行軸上の座標値より減じて前記目標位置を修正させることを特徴とする請求項５記載のコンピュータプログラム。
【請求項１】
走行軸を冗長軸とし、垂直多関節型で６軸のアームを有するロボットの制御を行う制御装置において、
前記アームの先端について、現在位置と、所定時間後の目標移動量とに基づいて決定される目標位置に対し、
その時点で前記アームが保持している第２関節角度θ２と第３関節角度θ３との和が一定となることを拘束条件として、前記アームの先端を前記目標位置に移動させるために必要な、前記走行軸上における前記所定時間当たりの移動量を計算することを特徴とするロボット制御装置。
【請求項２】
前記走行軸上における前記所定時間当たりの移動量に基づいて移動速度を求め、前記移動速度が上限速度を超える場合には、上記拘束条件を解除して前記移動速度を上限速度に置き換えると共に、両速度の差に応じた移動量を前記走行軸上の座標値より減じて前記目標位置を修正することを特徴とする請求項１記載のロボット制御装置。
【請求項３】
走行軸を冗長軸とし、垂直多関節型で６軸のアームを有するロボットの制御を行う制御方法において、
前記アームの先端について、現在位置と、所定時間後の目標移動量とに基づいて決定される目標位置に対し、
その時点で前記アームが保持している第２関節角度θ２と第３関節角度θ３との和が一定となることを拘束条件として、前記アームの先端を前記目標位置に移動させるために必要な、前記走行軸上における前記所定時間当たりの移動量を計算することを特徴とするロボット制御方法。
【請求項４】
前記走行軸上における前記所定時間当たりの移動量に基づいて移動速度を求め、
前記移動速度が上限速度を超える場合には、上記拘束条件を解除して前記移動速度を上限速度に置き換え、
両速度の差に応じた移動量を前記走行軸上の座標値より減じて前記目標位置を修正することを特徴とする請求項３記載のロボット制御方法。
【請求項５】
走行軸を冗長軸とし、垂直多関節型で６軸のアームを有するロボットの制御を行う制御装置を構成するコンピュータにより実行されるプログラムにおいて、
前記アームの先端について、現在位置と、所定時間後の目標移動量とに基づいて決定される目標位置に対し、
その時点で前記アームが保持している第２関節角度θ２と第３関節角度θ３との和が一定となることを拘束条件として、前記アームの先端を前記目標位置に移動させるために必要な、前記走行軸上における前記所定時間当たりの移動量を計算させることを特徴とするコンピュータプログラム。
【請求項６】
前記走行軸上における前記所定時間当たりの移動量に基づいて移動速度を求めさせ、
前記移動速度が上限速度を超える場合には、上記拘束条件を解除して前記移動速度を上限速度に置き換えさせ、
両速度の差に応じた移動量を前記走行軸上の座標値より減じて前記目標位置を修正させることを特徴とする請求項５記載のコンピュータプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【公開番号】特開２００６−２６００１０（Ｐ２００６−２６００１０Ａ）
【公開日】平成１８年９月２８日（２００６．９．２８）
【国際特許分類】
【出願番号】特願２００５−７５０２６（Ｐ２００５−７５０２６）
【出願日】平成１７年３月１６日（２００５．３．１６）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【Ｆターム（参考）】
【公開日】平成１８年９月２８日（２００６．９．２８）
【国際特許分類】
【出願日】平成１７年３月１６日（２００５．３．１６）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【Ｆターム（参考）】
[ Back to top ]