(57)【特許請求の範囲】
【請求項１】
ロボットヘッドを備えたロボットの動作を互いに直交するＸ軸、Ｙ軸、Ｚ軸からなるロボット座標系において制御する制御手段と、操作者によって保持されるとともに、該操作者によって操作される制御スイッチが設けられた操作ヘッドと、互いに直交するＸ軸、Ｙ軸、Ｚ軸からなる基準座標系における上記操作ヘッドの位置および姿勢を検出するセンサとを備え、
上記制御手段が、上記制御スイッチが入れられている間、上記センサを介して上記基準座標系における操作ヘッドの位置および姿勢を認識して、上記ロボット座標系におけるロボットヘッドの位置と姿勢を、操作ヘッドの位置と姿勢の変化に追従させるようにしたロボット制御システムであって、
上記制御手段は、上記操作ヘッドの所定の姿勢を当該操作ヘッドの上記基準座標系における基準姿勢として設定してあり、
上記センサを介して上記制御スイッチが入れられた際における操作ヘッドの初期姿勢の上記基準姿勢に対する姿勢ベクトルを認識するとともに、
上記初期姿勢から変化した変化後の姿勢の上記基準姿勢に対する姿勢ベクトルを認識し、
これら操作ヘッドの初期姿勢の姿勢ベクトルと変化後の姿勢の姿勢ベクトルとの差から、上記初期姿勢からの姿勢の変化を示す変位ベクトルを求め、
さらに、該変位ベクトルと初期姿勢の姿勢ベクトルとを加算し、ここから初期姿勢の姿勢ベクトルを減算することにより、該初期姿勢からの変位ベクトルを上記基準姿勢からの姿勢の変化を示す変位ベクトルへと変換して、
この変換された基準姿勢からの変位ベクトルに、上記ロボット座標系におけるロボットヘッドの姿勢を追従させることによって、操作者がどのような姿勢で操作ヘッドを保持して制御スイッチを入れた場合でも、上記基準姿勢からの姿勢の変化として認識して、該基準姿勢からの変位ベクトルにロボットヘッドの姿勢を追従させるようにしたことを特徴とするロボット制御システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明はロボット制御システムに関し、詳しくはロボットヘッドの位置および姿勢を操作ヘッドの位置および姿勢に追従させるようにしたロボット制御システムに関する。
【背景技術】
【０００２】
従来、産業用ロボット等の動作を制御するロボットの制御システムとして、以下のようなものが知られている。
先端に手首１１の設けられたロボット１と、操作者が保持する操作グリップ４とを備え、操作者が操作グリップ４の位置および姿勢を変化させるのを相対位置姿勢計測装置３によって計測し、これによりロボット１の制御装置２がロボット１を制御して、上記手首１１の位置および姿勢を上記操作グリップ４の位置および姿勢に追従させるようにしたものが知られている。（特許文献１）
上記操作グリップ４には動作スイッチ４２が設けられており、操作者が動作スイッチ４２を入れている間だけ、上記相対位置姿勢計測装置３が操作グリップ４の位置および姿勢を計測し、制御装置２によってロボット１の動作を制御するようになっている。
【特許文献１】登録２７７６４７７号
【発明の開示】
【発明が解決しようとする課題】
【０００３】
ここで、上記特許文献１で用いられている６自由度位置姿勢センサ４１は操作グリップ４の位置と、操作グリップ４の姿勢とを別々に認識するものであり、このため上記手首１１の位置は、操作グリップ４の姿勢にかかわらず、操作者に装着された磁場発生装置３１からの相対位置に応じて移動する。
一方、操作グリップ４の姿勢は、上記６自由度位置姿勢センサ４１によって検出されるが、上記操作グリップ４の形状はロボット１の手首１１の形状に模してあることから、操作者はロボット１の手首１１の姿勢にあわせて操作グリップを把持する。
したがって、操作者が操作グリップ４を水平方向に揺動させると、ロボット１の手首１１もこれに追従して水平方向に揺動することとなる。
しかしながら、操作グリップ４がロボット１の手首１１に模されていない場合、以下のような問題が生じることとなる。
つまり、操作グリップが縦方向で保持されるのを前提に製造されているにもかかわらず、操作者が当該操作グリップを横方向に保持してしまうと、６自由度位置姿勢センサは操作グリップの位置については手首の位置を追従させて移動させるものの、操作者が操作グリップを水平方向に揺動させると、手首１１は上下方向に揺動してしまい、正確にロボット１の姿勢を追従させることができないといった問題が生じてしまう。
このような問題に鑑み、操作者がどのような姿勢で操作グリップを保持しても、操作グリップの位置および姿勢に対して、ロボットヘッドの位置および姿勢を追従させることの可能なロボット制御システムを提供するものである。
【課題を解決するための手段】
【０００４】
すなわち、本発明におけるロボット制御システムは、ロボットヘッドを備えたロボットの動作を互いに直交するＸ軸、Ｙ軸、Ｚ軸からなるロボット座標系において制御する制御手段と、操作者によって保持されるとともに、該操作者によって操作される制御スイッチが設けられた操作ヘッドと、互いに直交するＸ軸、Ｙ軸、Ｚ軸からなる基準座標系における上記操作ヘッドの位置および姿勢を検出するセンサとを備え、
上記制御手段が、上記制御スイッチが入れられている間、上記センサを介して上記基準座標系における操作ヘッドの位置および姿勢を認識して、上記ロボット座標系におけるロボットヘッドの位置と姿勢を、操作ヘッドの位置と姿勢の変化に追従させるようにしたロボット制御システムであって、
上記制御手段は、上記操作ヘッドの所定の姿勢を当該操作ヘッドの上記基準座標系における基準姿勢として設定してあり、
上記センサを介して上記制御スイッチが入れられた際における操作ヘッドの初期姿勢の上記基準姿勢に対する姿勢ベクトルを認識するとともに、
上記初期姿勢から変化した変化後の姿勢の上記基準姿勢に対する姿勢ベクトルを認識し、
これら操作ヘッドの初期姿勢の姿勢ベクトルと変化後の姿勢の姿勢ベクトルとの差から、上記初期姿勢からの姿勢の変化を示す変位ベクトルを求め、
さらに、該変位ベクトルと初期姿勢の姿勢ベクトルとを加算し、ここから初期姿勢の姿勢ベクトルを減算することにより、該初期姿勢からの変位ベクトルを上記基準姿勢からの姿勢の変化を示す変位ベクトルへと変換して、
この変換された基準姿勢からの変位ベクトルに、上記ロボット座標系におけるロボットヘッドの姿勢を追従させることによって、操作者がどのような姿勢で操作ヘッドを保持して制御スイッチを入れた場合でも、上記基準姿勢からの姿勢の変化として認識して、該基準姿勢からの変位ベクトルにロボットヘッドの姿勢を追従させるようにしたことを特徴としている。
【発明の効果】
【０００５】
上記発明によれば、操作者がどのような姿勢で操作ヘッドを保持しても、制御手段は制御スイッチが入れられた際の操作ヘッドの初期姿勢を認識し、その後操作ヘッドの姿勢が変化すると、操作ヘッドの姿勢の変化として、上記初期姿勢からの姿勢の変位ベクトルを求める。
このとき上記初期姿勢が上記基準姿勢とは異なる姿勢であっても、制御手段が上記初期姿勢からの姿勢の変位ベクトルを当該基準姿勢からの姿勢の変位ベクトルへと変換するので、操作者が操作ヘッドを傾けた方向に、ロボットヘッドが傾くこととなる。
つまり、操作者がどのような姿勢で操作ヘッドを保持しても、その後のロボットヘッドの姿勢は初期姿勢からの変化ではなく、常に基準姿勢からの変化として認識されるので、操作ヘッドの姿勢にかかわらず、操作ヘッドの位置および姿勢の変化に対して、ロボットヘッドの位置および姿勢を追従させることができる。
【発明を実施するための最良の形態】
【０００６】
以下図示実施例について説明すると、図１は、従来公知の産業用のロボット１と、当該ロボット１を制御するロボット制御システム２とを示している。
上記ロボット１は６軸多関節型のロボット１であり、その先端には物品を把持するグリッパ等のロボットヘッド１ａが設けられている。このロボットヘッド１ａとしては、グリッパの他、塗装用の噴射手段や、溶接用のガン等を装着することができる。
ロボット制御システム２は、操作者が保持する操作ヘッド３と、所定の基準座標系に対する当該操作ヘッド３の位置および姿勢を検出するセンサ４と、上記センサ４が検出した操作ヘッド３の位置および姿勢の変化を認識すると共に、上記ロボット１の動作を制御する制御手段５とを備えている。
そして本実施例のロボット制御システム２では、操作者が操作ヘッド３の位置および姿勢を変化させると、センサ４によってその位置および姿勢の変化が検出され、制御手段５は操作ヘッド３の位置および姿勢の変化にロボットヘッド１ａの位置および姿勢が追従するよう、ロボット１の制御を行う。
【０００７】
図２は上記操作ヘッド３を示す図であり、操作ヘッド３は操作者によって把持可能な略直方体のケース１１と、当該ケース１１の頂部に設けられた制御スイッチ１２とから構成され、この制御スイッチ１２は操作者に押されている間だけ、制御手段５に信号を送信するようになっている。
次に、上記センサ４は、図１に示すように操作ヘッド３を保持する操作者の隣接位置に設置される原点部１３と、図２に示すように上記操作ヘッド３のケース１１内に設けられた移動部１４と、これら原点部１３と移動部１４とが接続される本体部１５とを備えている。
上記原点部１３は、図示しない３つのコイルを備えており、各コイルは、水平に向けられたＸ軸方向、水平に向けられてＸ軸方向に直交するＹ軸方向、鉛直方向に向けられたＺ軸方向のそれぞれを向いており、これらＸ軸、Ｙ軸、Ｚ軸の各座標軸により、これらの座標軸の交点を原点とする所定の基準座標系としての基準座標系Ｂが設定されている。
該基準座標系Ｂには、各座標軸を回転中心とする回転座標が設定されており、Ｘ軸を中心に回転するΨ座標、Ｙ軸を中心に回転するΘ座標、Ｚ軸を中心に回転するΦ座標がそれぞれ設定されている。そしてこの基準座標系Ｂを基準として操作ヘッド３の位置および姿勢を検出するようになっている。
そして、上記ロボット１にはロボット座標系Ｒが設定されており、上記ロボットヘッド１ａの位置および姿勢はこのロボット座標系Ｒに基づいて変化し、本実施例では上記基準座標系Ｂを該ロボット座標系Ｒと平行に設定している。
【０００８】
次に、上記移動部１４にも図示しない３つのコイルが設けられており、これらのコイルは上記原点部１３と同様、原点にて相互に直交する方向に向けられている。
また、移動部１４の下部には、移動部１４を上記ケース１１に固定するための固定片１４ａが両側に伸びて設けられており、本実施例の場合、当該固定片１４ａが伸びる方向をＹ軸とし、Ｙ軸と直交する移動部１４の上下方向をＺ軸とし、これらＹ，Ｚ軸と直交する方向をＸ軸としている。
そして、この移動部１４の３つのコイルによるＸＹＺ軸の向きと原点部１３の３つのコイルによるＸＹＺ軸の向きとが一致するときの操作ヘッド３の姿勢、すなわち基準座標系ＢのＸＹＺ軸の向きと、移動部１４のＸＹＺ軸の向きとが一致する時の姿勢、さらには移動部１４のＸＹＺ軸の向きとロボット座標系Ｒとが一致する姿勢を、以下、操作ヘッド３の基準姿勢と呼ぶ。
上記本体部１５は、上記原点部１３に電力を供給して上記３つのコイルから磁場を発生させ、上記移動部１４からは、この磁場によって該移動部１４の各コイルに生じた誘導電流を検出して、原点部１３に設定された基準座標系Ｂに対する移動部１４の位置および姿勢が検出される。
なお、本実施例で使用するセンサ４は、上記特許文献１等にも記載されている６自由度磁気センサであり、従来公知であるので、これ以上の詳細な説明は省略する。
【０００９】
上記制御手段５は、上記センサ４が検出した操作ヘッド３の位置および姿勢から、ロボット１の制御に必要な信号を出力する指令出力部２１と、指令出力部２１からの信号に従ってロボット１の制御を行うことで、ロボットヘッド１ａの位置および姿勢を操作ヘッド３の位置および姿勢の変化に追従させるロボットコントローラ２２とを備えている。
さらに、上記指令出力部２１には、上記センサ４が検出した移動部１４の位置の変化を認識する位置認識部２３と、移動部１４の姿勢の変化を認識する姿勢認識部２４と、姿勢認識部２４が認識した操作ヘッド３の姿勢の変化にロボットヘッド１ａの姿勢を追従させるための姿勢の変換を行う姿勢変換部２５とが備えられている。
上記位置認識部２３は、原点部１３に設定された基準座標系Ｂに対する移動部１４の原点が位置する座標値を位置ベクトルとして認識するようになっており、上記操作者が操作ヘッド３の制御スイッチ１２を入れたときの位置ベクトルを初期位置として認識し、また操作ヘッド３の位置が変化すると、変化する位置ベクトルの差を求めて、位置の変化を示す変位ベクトルとして認識するようになっている。
上記姿勢認識部２４は、移動部１４の姿勢の変化による上記基準座標系ＢのＸＹＺ軸に対する移動部１４のＸＹＺ軸の角度差を姿勢ベクトルとして認識するようになっており、上記操作者が操作ヘッド３の制御スイッチ１２を入れたときの姿勢ベクトルを初期姿勢として認識し、また操作ヘッド３の姿勢が変化すると、変化する姿勢ベクトルの差を求めて、初期姿勢からの姿勢の変化を示す変位ベクトルとして認識するようになっている。
姿勢変換部２５は、後に詳述するように、上記姿勢認識部２４が認識した上記初期姿勢からの姿勢の変位ベクトルを、上記基準姿勢からの変位ベクトルに変換するようになっている。
ロボットコントローラ２２は、操作者が制御スイッチ１２を押した時のロボットヘッド１ａの位置および姿勢を、上記ロボット座標系Ｒを基準とする動作開始点として認識し、その後上記位置認識部２３が認識した位置の変位ベクトルに応じてロボットヘッド１ａの位置を変化させ、上記姿勢変換部２５が変換した基準姿勢からの姿勢の変位ベクトルに応じてロボットヘッド１ａの姿勢を変化させるよう、ロボット１の制御を行う。
そして操作者が制御スイッチ１２を切ることで、ロボットコントローラ２２は直ちにロボット１の作動を停止させるようになっている。
【００１０】
以下、上記構成を有するロボット制御システム２によるロボット１の制御について説明する。なお、上記基準座標系Ｂの原点位置の位置ベクトルを（Ｘ０，Ｙ０，Ｚ０）、原点姿勢の姿勢ベクトルを（Ψ０，Θ０，Φ０）として説明する。
操作者が操作ヘッド３を保持し、制御スイッチ１２を入れると、センサ４では原点部１３に対する移動部１４の位置および姿勢を検出して、指令出力部２１の位置認識部２３は操作ヘッド３の初期位置の位置ベクトル（Ｘ１，Ｙ１，Ｚ１）を認識し、姿勢認識部２４は操作ヘッド３の初期姿勢の姿勢ベクトル（Ψ１，Θ１，Φ１）を認識する。
そして、姿勢変換部２５には、予め基準姿勢が設定されており、この操作ヘッド３の基準姿勢は、操作ヘッド３が上記図２で示したように移動部１４内の３つのコイルの向きが原点部１３内の３つのコイルの向きと一致する姿勢として設定されている。すなわち、基準姿勢は原点姿勢と同じ姿勢ベクトル（Ψ０，Θ０，Φ０）で表すことができる。
そしてロボットコントローラ２２では、上記ロボットヘッド１ａの位置および姿勢にかかわらず、制御スイッチ１２が入れられた時のロボットヘッド１ａの位置および姿勢を動作開始点とする。
【００１１】
そして操作者が制御スイッチ１２を押したまま操作ヘッド３の位置および姿勢を変化させると、センサ４では所定時間間隔で移動部１４の位置および姿勢を検出し、制御スイッチ１２が押された直後にセンサ４が検出した操作ヘッド３の位置および姿勢は以下のように処理される。
位置認識部２３では、上記初期位置（Ｘ１，Ｙ１，Ｚ１）から所定時間経過した移動後の移動部１４の位置ベクトル（Ｘ２，Ｙ２，Ｚ２）を認識し、この位置ベクトル（Ｘ２，Ｙ２，Ｚ２）から上記初期位置（Ｘ１，Ｙ１，Ｚ１）を減算して、位置の変位ベクトル（ｘ２、ｙ２、ｚ２）を算出する。
姿勢認識部２４では、上記初期姿勢（Ψ１，Θ１，Φ１）に対する移動後の移動部１４の姿勢ベクトル（Ψ２，Θ２，Φ２）を認識し、この姿勢ベクトル（Ψ２，Θ２，Φ２）から上記初期姿勢（Ψ１，Θ１，Φ１）を減算して、初期姿勢からの姿勢の変位ベクトル（ψ２、θ２、φ２）を算出する。
姿勢変換部２５では、この初期姿勢に対する姿勢の変位ベクトル（ψ２、θ２、φ２）と初期姿勢ベクトル（Ψ１，Θ１，Φ１）との和を求め、ここからさらに初期姿勢ベクトル（Ψ１，Θ１，Φ１）を減算することで、原点姿勢からの変位ベクトルが算出される。
この原点姿勢からの変位ベクトルは、換言すれば基準姿勢からの変位ベクトルをも示しており、姿勢変換部２５はここで算出した変位ベクトルを基準姿勢からの変位ベクトルとして認識する。
つまり、操作ヘッド３の初期姿勢の姿勢ベクトルから次の移動後の姿勢ベクトルを減算することで、初期姿勢から所定時間経過して変化した姿勢の変位ベクトルが算出され、この姿勢の変位ベクトルに初期姿勢の姿勢ベクトルを一旦加えることで、基準座標系Ｂを基準とする原点姿勢からの姿勢の変化が加味される。
そしてこの姿勢の変位ベクトルと初期姿勢の姿勢ベクトルとを加算したものから、再び初期姿勢の姿勢ベクトルを減算すると、原点姿勢からの差がインバース、すなわち、逆転されるので、原点姿勢からの変位ベクトルとして算出され、これにより基準姿勢からの姿勢の変化を認識することができる。
そして位置認識部２３および姿勢変換部２５は、ロボットコントローラ２２に位置の変位ベクトルおよび基準姿勢に対する姿勢の変位ベクトルを送信し、ロボットコントローラ２２は、ロボット座標系Ｒを基準に、ロボットヘッド１ａの位置および姿勢をこれら各変位ベクトルに対応させるよう、ロボット１の制御を行う。
【００１２】
このとき、ロボットヘッド１ａの姿勢は基準姿勢からの姿勢の変化として、該基準姿勢からの変位ベクトルに応じて変化するが、この姿勢が変化する方向は、操作者が操作ヘッド３を傾けた方向に一致することとなる。
これを図３を用いて説明すると、図３（ａ）は移動部１４が基準姿勢である場合の図を、図３（ｂ）は操作者が操作ヘッド３をＹ軸を中心に９０°回転させた状態で上記制御スイッチ１２を入れた時の初期姿勢を示している。
本実施例では、上記姿勢認識部２４によって図３（ｂ）の初期姿勢が認識され、その後移動部１４の姿勢の変化が認識されると、姿勢認識部２４は認識した姿勢の変化に応じて初期姿勢からの変位ベクトルを求め、姿勢変換部２５では初期姿勢からの変位ベクトルを基準姿勢（破線で示す）からの姿勢の変化を示す変位ベクトルへと変換させる。
すなわち、操作ヘッド３を横方向の軸を中心に回動させると、図３（ａ）の場合、移動部１４はＸ軸を中心に（ψ１、０，０）だけ回転し、図３（ｂ）の場合、実際の操作ヘッド３の動きはＺ軸を中心として回転されるが、制御上は移動部１４が基準姿勢からの姿勢の変化として、実際の動きと同じ方向および変化量にて基準姿勢から姿勢を変化させるので、Ｘ’軸を中心に（ψ１’、０、０）だけ回転するよう認識されることとなる。
その結果、（ａ）、（ｂ）いずれの場合も上記ロボットコントローラ２２はロボットヘッド１ａをロボット座標系ＲにおけるＸ軸を中心に（ψ１、０，０）だけ回転し、操作者が操作ヘッド３の姿勢を変化させる動作と同じように、ロボットヘッド１ａの姿勢が追従して、横方向を向いた軸（Ｘ軸、Ｘ’軸）を中心として回転するように変化する。
これに対し、姿勢変換部２５が備えられていない場合には、操作ヘッド３の動きのまま移動部１４はＺ軸を中心に（０，０、φ１）だけ回転して認識される。
このため、ロボットコントローラ２２はロボットヘッド１ａをロボット座標系ＲにおけるＺ軸を中心に（０、０，φ１）だけ回転させることとなり、Ｚ軸は鉛直方向を向いていることから、ロボットヘッド１ａは操作者のイメージに反して上下方向を向いた軸（Ｚ軸）を中心に水平方向に回転してしまう。
このように、上記姿勢変換部２５によって、常に基準姿勢からの姿勢の変化に変換する事で、操作者がどのような姿勢で操作ヘッド３を保持しても、操作者がイメージする位置および姿勢の変化にロボットヘッド１ａの位置および姿勢を追従させることができる。
【００１３】
操作者が制御スイッチ１２を押し、操作ヘッド３の位置および姿勢を変化させてから、最後に操作者が制御スイッチ１２を離すまでは、所定の時間間隔で以下のように繰り返し処理される。
位置認識部２３では、今回求めた位置ベクトルからその直前に検出された前回の位置ベクトルを減算して、初期位置から引き続く、今回の位置の変化を示す変位ベクトルを算出する。
また姿勢認識部２４では、今回求めた姿勢ベクトルからその直前に検出された前回の姿勢ベクトルを減算することで、初期姿勢から引き続く、今回の姿勢の変化を示す姿勢の変位ベクトルを算出する。
そして姿勢変換部２５では、姿勢認識部２４で算出された上記姿勢の変位ベクトルを入力すると、該変位ベクトルと上記前回の姿勢ベクトルとの和を求め、ここから該前回の姿勢ベクトルを減算することで、基準姿勢からの変位ベクトルへと変換する。
なお、この姿勢変換部２５による変位ベクトルの変換の手順として、今回の姿勢の変化により算出された変位ベクトルを初期姿勢からの姿勢の変化と仮定することが可能である。つまり、変位ベクトルと初期姿勢の姿勢ベクトルとの和から初期姿勢の姿勢ベクトルを減算すれば、上述した変換と同様の結果を得ることができる。また、初期姿勢の姿勢ベクトルに代えて、別途定めた所定のベクトルをあてることもできる。
そして位置認識部２３および姿勢変換部２５は、ロボットコントローラ２２に求めた位置の変位ベクトルおよび基準姿勢からの姿勢変位ベクトルを送信し、ロボットコントローラ２２は、直前のロボットヘッド１ａの位置および姿勢を、これら各変位ベクトルに追従して変化させるよう、ロボット１の制御を行う。
これにより、制御スイッチ１２が入れられた操作ヘッド３の初期状態の位置および姿勢から、その後の操作ヘッド３の位置および姿勢の変化に、、ロボットヘッド１ａの位置と姿勢を追従させて変化させることができるようになっている。
【００１４】
なお、本実施例では操作ヘッド３の位置および姿勢を変化させると直ちにロボットヘッド１ａの位置および姿勢も変化させていたが、操作ヘッド３の位置および姿勢の変化を上記ロボットコントローラ２２等に記憶させるようにしても良く、いわゆるロボット１のティーチングに利用することができる。
【図面の簡単な説明】
【００１５】
【図１】本実施例に係るロボット制御システム２の構成図。
【図２】操作ヘッド３についての拡大図。
【図３】姿勢変換部２５の有無による比較を説明する図であって、（ａ）は基準姿勢どおりに操作ヘッド３を把持した場合を、（ｂ）は基準姿勢に対して９０°回転した状態で操作ヘッド３を把持した場合をしめす。
【符号の説明】
【００１６】
１  ロボット                  １ａ  ロボットヘッド
２  ロボット制御システム      ３  操作ヘッド
４  センサ                    ５  制御手段
１２制御スイッチ              １３  原点部
１４  移動部                  ２３  位置認識部
２４  姿勢認識部              ２５  姿勢変換部
【図１】
0004845431.tif 000002
【図２】
0004845431.tif 000003
【図３】
0004845431.tif 000004