ロボットの制振方法およびロボットの制御装置
【課題】ロボットの先端の振動を抑制する。
【解決手段】複数のリンク２４〜３４と対応するリンクを駆動する複数のモータ３６〜４６とを有するロボットの動作が停止するときに発生するロボット先端の振動を抑制するロボットの制振方法であって、ロボット先端の加速度を加速度センサ５２によって検出し、加速度センサが検出した加速度に基づいてロボット先端の速度を算出する。ロボット先端の速度と、加速度センサの加速度検出タイミングにおける複数のモータの回転角度と、ヤコビ行列Ｊの逆行列Ｊ−１とに基づいて、各リンクと対応するモータとの間のねじれ角速度を算出する。さらに、ねじれ角速度に基づいてねじれ角度を算出し、ねじれ角速度とねじれ角度とに基づいて、各リンクと対応するモータとの間に発生したねじれが解消する、各モータの制御入力に加算する補償量を算出し、補償量を加算した制御入力を各モータに出力する。
【解決手段】複数のリンク２４〜３４と対応するリンクを駆動する複数のモータ３６〜４６とを有するロボットの動作が停止するときに発生するロボット先端の振動を抑制するロボットの制振方法であって、ロボット先端の加速度を加速度センサ５２によって検出し、加速度センサが検出した加速度に基づいてロボット先端の速度を算出する。ロボット先端の速度と、加速度センサの加速度検出タイミングにおける複数のモータの回転角度と、ヤコビ行列Ｊの逆行列Ｊ−１とに基づいて、各リンクと対応するモータとの間のねじれ角速度を算出する。さらに、ねじれ角速度に基づいてねじれ角度を算出し、ねじれ角速度とねじれ角度とに基づいて、各リンクと対応するモータとの間に発生したねじれが解消する、各モータの制御入力に加算する補償量を算出し、補償量を加算した制御入力を各モータに出力する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの振動を抑制する制振方法およびロボットの制御装置に関する。
【背景技術】
【０００２】
従来より、先端に溶接ガンやスプレーガンなどのツールを取付けて動作する多関節ロボットは、動作の停止直後にその先端が振動するという問題がある。これは、ツールやロボットの先端部分の慣性により、ロボットのリンクが、片持ち梁のように振動することにより起こる。
【０００３】
このリンクの振動（先端の振動）に対処する方法として、例えば特許文献１に記載する方法がある。この特許文献１に記載する方法は、加速度センサをロボットの先端に取付け、その加速度センサが検出した先端の加速度に基づいてリンクを駆動するモータを制御することにより、リンクを制振し、ロボットの先端の振動を抑制する。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平４−１９３４９０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、上述の特許文献１の場合、加速度センサがロボットの先端の加速度を検出してから、その検出結果に基づいてリンクを制振するまでに大きな時間遅れが生じる。具体的に説明すると、加速度センサが検出したロボットの先端の加速度は、速度演算、座標演算を順に経てロボットの先端の位置変化量に演算される。この位置変化量に基づいてリンクを制振できる位置変更指令値が算出され、その位置変更指令値はリンクを駆動するモータに対する指令値に変換演算される。このように複数の演算を重ねるために、加速度センサがロボットの先端の加速度を検出してからモータの指令に反映されるまでの時間遅れが大きくなる。リンクの振動周期によっては（特に周期が短い場合）、この時間遅れによってリンクを十分に制振できず、最悪な場合、ロボットの先端の振動が長期化する可能性がある。
【０００６】
そこで、本発明は、ロボットの動作の停止直後において、加速度センサロボットの先端の加速度を検出してからすぐに、その加速度に基づいてリンクを駆動するモータを制御することにより、リンクを十分に制振し、先端の振動を抑制することを課題とする。
【課題を解決するための手段】
【０００７】
上述の課題を解決するために、本発明の請求項１に係る発明は、
複数のリンクと対応するリンクを駆動する複数のモータとを有するロボットの動作が停止するときに発生するロボット先端の振動を抑制するロボットの制振方法であって、
ロボット先端の加速度を加速度センサによって検出し、
加速度センサが検出した加速度に基づいてロボット先端の速度を算出し、
ロボット先端の速度と、加速度センサの加速度検出タイミングにおける複数のモータの回転角度と、ヤコビ行列の逆行列とに基づいて、各リンクと対応するモータとの間のねじれ角速度を算出し、
ねじれ角速度に基づいてねじれ角度を算出し、
ねじれ角速度とねじれ角度とに基づいて、各リンクと対応するモータとの間に発生したねじれが解消する、各モータの制御入力に加算する補償量を算出し、
補償量を加算した制御入力を各モータに出力することを特徴とする。
【０００８】
また、請求項２に係る発明は、
複数のリンクと対応するリンクを駆動する複数のモータとを有するロボットの動作が停止するときに発生するロボット先端の振動を抑制することが可能なロボットの制御装置であって、
ロボットの外部に予め定義されたＸＹＺ直交座標系と、
ロボット先端のＸ，Ｙ，およびＺ軸方向の加速度とＸ，Ｙ，およびＺ軸まわりの加速度とを検出する加速度センサと、
対応するモータの回転角度を検出する複数のエンコーダと、
加速度センサが検出した加速度に基づいてロボット先端の速度を算出する速度算出手段と、
速度算出手段が算出した速度と、加速度センサの加速度検出タイミングにおいて複数のエンコーダが検出した回転角度と、ヤコビ行列の逆行列とに基づいて、各リンクと対応するモータとの間のねじれ角速度を算出するねじれ角速度算出手段と、
ねじれ角速度算出手段が算出したねじれ角速度に基づいて、ねじれ角度を算出するねじれ角度算出手段と、
ねじれ角速度算出手段が算出したねじれ角速度と、ねじれ角度算出手段が算出したねじれ角度とに基づいて、各リンクと対応するモータとの間に発生したねじれが解消する、各モータの制御入力に加算する補償量を算出する補償量算出手段と、
補償量を加算した制御入力を各モータに出力するモータ制御手段とを有することを特徴とする。
【０００９】
さらに、請求項３に係る発明は、
３つのリンクと対応するリンクを駆動する３つのモータとを有するロボットの動作が停止するときに発生するロボット先端の振動を抑制することが可能なロボットの制御装置であって、
ロボットの外部に予め定義されたＸＹＺ直交座標系と、
ロボット先端のＸ，Ｙ，およびＺ軸方向の加速度を検出する加速度センサと、
対応するモータの回転角度を検出する複数のエンコーダと、
加速度センサが検出した加速度に基づいてロボット先端の速度を算出する速度算出手段と、
速度算出手段が算出した速度と、加速度センサの加速度検出タイミングにおいて複数のエンコーダが検出した回転角度と、ヤコビ行列の逆行列とに基づいて、各リンクと対応するモータとの間のねじれ角速度を算出するねじれ角速度算出手段と、
ねじれ角速度算出手段が算出したねじれ角速度に基づいてねじれ角度を算出するねじれ角度算出手段と、
ねじれ角速度算出手段が算出したねじれ角速度と、ねじれ角度算出手段が算出したねじれ角度とに基づいて、各リンクと対応するモータとの間に発生したねじれが解消する、各モータの制御入力に加算する補償量を算出する補償量算出手段と、
補償量を加算した制御入力を各モータに出力するモータ制御手段とを有することを特徴とする。
【００１０】
さらにまた、請求項４に係る発明は、
ロボットは、複数のリンクと対応するリンクを駆動する複数のモータとを有し、
上記３つのリンクは、複数のリンクの一部であって、他のリンクに比べて長いことを特徴とする。
【発明の効果】
【００１１】
本発明によれば、ロボットの動作の停止直後、加速度センサが検出した加速度からロボット先端の速度が算出され、その速度とヤコビ行列の逆行列とに基づいてリンクとモータとの間のねじれ角速度が算出される。次にねじれ角速度に基づいてねじれ角度が算出され、ねじれ角速度とねじれ角度に基づいて複数のモータそれぞれの制御入力に加算される、ねじれを解消する補償量が算出される。そして、補償量はそれぞれ、対応するモータの制御入力に加算される。
【００１２】
これにより、加速度センサが検出した加速度からモータの制御入力の補償量が短時間で算出され、その補償量が加算された制御入力によってモータが対応するリンクとの間のねじれを解消するためにすばやく回転する。したがって、加速度センサが検出した加速度からロボット先端の位置変化量を求め、位置変化量からリンクを制振できる位置姿勢変更指令値を算出し、その位置変更指令値をモータの指令値に演算してリンクを制振する場合に比べて、ロボット先端の加速度を検出してからリンクが制振されるまでの時間が短くなる。その結果、リンクを十分に制振でき、ロボット先端の振動を抑制することができる。
【図面の簡単な説明】
【００１３】
【図１】本発明の第１の実施形態に係るロボットのリンクモデルを示す図である。
【図２】リンクの振動を説明するための図である。
【図３】本発明の第１の実施形態に係るロボットの制御装置の一部のブロック図である。
【図４】ヤコビ行列の部分行列を説明するための図である。
【図５】ヤコビ行列の部分行列を説明するための別の図である。
【図６】第２の実施形態に係るロボットの制御装置の一部のブロック図である。
【発明を実施するための形態】
【００１４】
（第１の実施形態）
本発明の第１の実施形態に係るロボットのリンクモデルを図１に示す。図１に示すロボット１０は、６つの関節１２〜２２を備え、複数のリンク２４〜３４と対応するリンクを駆動する複数のモータ３６〜４６（図３参照）とを有する。モータ３６はリンク２４を駆動し、モータ３８はリンク２６、モータ４０はリンク２８、モータ４２はリンク３０、モータ４４はリンク３２、モータ４６はリンク３４を駆動する。
【００１５】
また、ロボット１０は、先端４８に溶接ガン、スプレーガン、またはハンドツールなどのツール５０を備えている。
【００１６】
さらに、ロボット１０は、その先端４８の加速度を検出する６軸センサ５２と、対応するモータの回転角度を検出する複数のエンコーダ５４〜６４（図３参照）とを備えている。６軸センサ５２を用いて、ロボットの外部に予め定義されている固定の基準座標系ΣＢを基準としてロボット先端４８の加速度を検出する。具体的には、例えば、６軸センサ５２のセンサ座標系に関する出力を、公知の手段によって基準座標系ΣＢのＸ，Ｙ，およびＺ軸方向の加速度と、Ｘ，Ｙ，およびＺ軸まわりの加速度に変換する。エンコーダ５４はモータ３６の回転角度θ１を検出し、エンコーダ５６はモータ３８の回転角度θ２、エンコーダ５８はモータ４０の回転角度θ３、エンコーダ６０はモータ４２の回転角度θ４、エンコーダ６２はモータ４４の回転角度θ５、エンコーダ６４はモータ４６の回転角度θ６を検出する。
【００１７】
さらにまた、ロボット１０（ロボット１０の制御装置）は、ロボット１０の動作の停止直後に、複数のリンク２４〜３４を制振するように構成されている。すなわち、ロボット１０の動作の停止直後に、複数のリンク２４〜３４を振動しないようにする。
【００１８】
まず、複数のリンク２４〜３４の制振を説明する前に、ロボット１０の動作の停止直後に発生する複数のリンク２４〜３４の振動（制振しない場合に発生する振動）について、図２を参照しながら説明する。
【００１９】
リンクの振動の発生は、主に３つのケースに分けられる。他のリンクの動作が停止することによってリンクが振動し始めるケース（図２（Ａ））と、モータによる駆動が停止することによってリンクが振動し始めるケース（図２（Ｂ））と、これら２つを組み合わせたケースがある。
【００２０】
例えば、図２（Ａ）に示すように、他のリンクが動作することによってリンクＬがそのツール側端が位置Ｐ０に位置する状態から移動し始め、そのツール側端が位置Ｐ１に到達したときに他のリンクが停止すると、リンクＬよりツール側に位置する他のリンク、モータ、ツールなどの慣性により、リンクＬがたわむ。このリンクＬのたわみにより、リンクＬのツール側端が位置Ｐ１を通過して位置Ｐ２に位置し、リンクＬと該リンクＬと連結されたモータＭとの間にねじれが発生する（ねじれ角度αが発生する）。そして、リンクＬは、このねじれを原因として、ツール側端が位置Ｐ１で停止するまで、振動し続ける。なお、本明細書では、「ねじれ角度α」は、図に示すように、ツール側端が本来あるべき位置Ｐ１とモータＭの中心とを結ぶ直線と、ツール側端の現状の位置Ｐ２とモータＭの中心とを結ぶ直線との間の角度を言う。
【００２１】
また例えば、図２（Ｂ）に示すように、モータＭに駆動されることによってリンクＬがそのツール側端が位置Ｐ０に位置する状態から回転し始め、角度β回転した後に（ツール側端が位置Ｐ１に到達したときに）モータＭが停止すると、リンクＬよりツール側に位置する他のリンク、モータ、ツールなどの慣性により、リンクがたわむ。このリンクのたわみにより、リンクのツール側端が位置Ｐ１を通過して位置Ｐ２に位置し、リンクＬと該リンクＬと連結されたモータＭとの間にねじれが発生する（ねじれ角度αが発生する）。そして、リンクＬは、このねじれを原因として、ツール側端が位置Ｐ１で停止するまで、振動し続ける。
【００２２】
このようなリンクとモータとの間に発生するねじれを原因として発生する振動を抑制するために、ロボット１０の制御装置は、複数のモータ３６〜４６に対して制振制御を実行する。
【００２３】
モータ３６〜４６に対して制振制御を実行するための構成を図３に示す。
【００２４】
図３は、ロボット１０の制御装置の一部、複数のモータ３６〜４６の制振制御に関連する構成要素のブロック図である。
【００２５】
ロボット１０の制御装置は、図３に示すように、ロボット先端４８の速度を算出する第１フィルタ７０と、ヤコビ行列の逆行列を算出する逆行列演算部７２と、リンク２４〜３４とモータ３６〜４６との間のねじれ角速度を算出するねじれ角速度演算部７４と、リンク２４〜３４とモータ３６〜４６との間のねじれ角度を算出する積分器７６と、モータ３６〜４６の制御入力に加算する補償量を算出する補償量演算部７８と、制御入力をモータ３６〜４６に出力するモータ制御部８０とを有する。
【００２６】
第１フィルタ７０は、ロボット１０の停止直後に６軸センサ５２が検出したロボット先端４８のＸ軸方向の加速度
、Ｙ軸方向の加速度
、Ｚ軸方向の加速度
、Ｘ軸まわりの加速度
、Ｙ軸まわりの加速度
、およびＺ軸まわりの加速度
に基づいて、ロボット先端４８のＸ軸方向の速度
、Ｙ軸方向の速度
、Ｚ軸方向の速度
、Ｘ軸まわりの速度
、Ｙ軸まわりの速度
、Ｚ軸まわりの速度
を算出する。具体的には、第１フィルタ７０は、６軸センサ５２から出力された信号からＨＰＦ（Ｈｉｇｈ Ｐａｓｓ Ｆｉｌｔｅｒ）によってＤＣ成分をカットし、その後に積分演算することにより、加速度
から速度
を算出する。
【００２７】
逆行列演算部７２は、後述するねじれ角速度演算部７４がねじれ角速度を算出するにあたって必要なヤコビ行列Ｊの逆行列をＪ−１算出する。
【００２８】
具体的には、複数のモータ３６〜４６の回転角度θ１〜θ６を変数とする関数のヤコビ行列Ｊ（数式１）が予め定義されており（例えば、制御装置の記憶部に記憶されており）、逆行列演算部７２は、そのヤコビ行列Ｊに回転角度θ１〜θ６を代入し、その後にヤコビ行列Ｊの逆行列Ｊ−１を算出する。さらに具体的に言えば、逆行列演算部７２は、６軸センサ５２がロボット先端４８の加速度
を検出するタイミングにおける複数のモータ３６〜４６の回転角度θ１〜θ６に基づいて、ヤコビ行列Ｊの逆行列Ｊ−１を算出する。なお、複数のモータ３６〜４６の回転角度θ１〜θ６は、対応するモータの回転角度を検出する複数のエンコーダ５４〜６４から得られる。
【数１】
【００２９】
ねじれ角速度演算部７４は、リンク２４〜３４とモータ３６〜４６との間のねじれ角速度
を算出する。なお、
はリンク２４とモータ３６との間のねじれ角速度であって、
はリンク２６とモータ３８、
はリンク２８とモータ４０、
はリンク３０とモータ４２、
はリンク３２とモータ４４、
はリンク３４とモータ４６との間のねじれ角速度である。
【００３０】
具体的には、第１フィルタ７０が算出したロボット先端４８の速度
と、逆行列演算部７２が算出したヤコビ行列Ｊの逆行列Ｊ−１とに基づいて、数式２を用いて、ねじれ角速度
を算出する。
【数２】
【００３１】
第２フィルタ７６は、ねじれ角速度
に基づいて、リンク２４〜３４とモータ３６〜４６との間のねじれ角度α１〜α６を算出する。
【００３２】
具体的には、第２フィルタ７６は、ねじれ角速度
を積分することによってねじれ角度α１〜α６を算出する。
【００３３】
補償量演算部７８は、複数のモータ３６〜４６それぞれに対するモータ制御部８０の制御入力（電流指令）に加算する補償量を算出する。
【００３４】
複数のモータ３６〜４６の補償量Ｉ１〜Ｉ６は、駆動するリンクのイナーシャ、バネ定数、およびダンパと、自身のトルク定数に基づいて算出される。
【００３５】
具体的に説明すると、リンクのイナーシャをＨ、バネ定数をＫ、ダンパをＤ、リンクとモータとの間のねじれ角度をα、ねじれ角速度をΔαとすると、リンクの運動方程式は、数式３のように表現することができる。
【数３】
【００３６】
数式３において、ａは、リンクの加速度である。
【００３７】
リンクの加速度ａを実現するために必要なモータ電流ＩＭは、数式４のように表現することできる。なお、数式４のＫtはモータのトルク定数である。
【数４】
【００３８】
数式４で表現されるモータ電流ＩＭは求めるべき補償量Ｉでもあるので、数式４を数式３に代入すると、数式５に示す補償量Ｉの式が得られる。
【数５】
【００３９】
補償量演算部７８が算出した補償量Ｉ１〜Ｉ６それぞれが対応するモータの制御入力に加算されることにより、複数のモータ３６〜４６それぞれが対応するリンクとの間のねじれが解消するように回転する。これにより、ロボット１０の動作の停止直後において、複数のリンク２４〜３４の振動の発生が抑制される。なお、このときロボット１０の動作は停止しているが、その停止姿勢を維持するために必要な制御入力が、モータ制御部８０から複数のモータ３６〜４６それぞれに出力されている。
【００４０】
本実施形態によれば、ロボット１０の動作の停止直後、６軸センサ５２が検出した加速度
からロボット先端４８の速度
が算出され、その速度
とヤコビ行列Ｊの逆行列Ｊ−１とに基づいてリンク２４〜３４とモータ３６〜４６との間のねじれ角速度
が算出される。次にねじれ角速度
に基づいてねじれ角度α１〜α６が算出され、ねじれ角速度
とねじれ角度α１〜α６に基づいて複数のモータ３６〜４６それぞれの制御入力に加算される、ねじれを解消する補償量Ｉ１〜Ｉ６が算出される。そして、補償量Ｉ１〜Ｉ６はそれぞれ、対応するモータの制御入力に加算される。
【００４１】
これにより、６軸センサ５２が検出した加速度
からモータ３６〜４６の制御入力の補償量Ｉ１〜Ｉ６が短時間で算出され、その補償量Ｉ１〜Ｉ６が加算された制御入力によってモータ３６〜４６が対応するリンク２４〜３４との間のねじれを解消するためにすばやく回転する。したがって、加速度センサが検出した加速度からロボット先端の位置変化量を求め、位置変化量からリンクを制振できる位置姿勢変更指令値を算出し、その位置変更指令値をモータの指令値に演算してリンクを制振する場合に比べて、ロボット先端の加速度を検出してからリンクが制振されるまでの時間が短くなる。その結果、リンクを十分に制振でき、ロボット先端の振動を抑制することができる。
【００４２】
（第２の実施形態）
本実施形態は、第１の実施形態に比べてより現実的且つ経済的な形態である。
【００４３】
第１の実施形態の場合、ロボット１０の停止直後において、複数のリンク２４〜３４を制振するために、６軸センサ５２を使用している。すなわち、ロボット先端のＸ軸方向の加速度
、Ｙ軸方向の加速度
、Ｚ軸方向の加速度
、Ｘ軸まわりの加速度
、Ｙ軸まわりの加速度
、およびＺ軸まわりの加速度
を検出している。しかしながら、実際に問題になるリンクやロボット先端の振動は、Ｘ軸方向、Ｙ軸方向、Ｚ軸方向、またはこれらを組み合わせた方向の振動であるため、ロボット先端の加速度としてＲｘ，Ｒｙ，Ｒｚ成分を検出する必要は現実的にはあまりない。したがって、本実施形態は、第１の実施形態と異なり、ロボット先端のＸ軸方向の加速度
、Ｙ軸方向の加速度
、Ｚ軸方向の加速度
を検出する３軸センサを使用する。３軸センサは６軸センサに比べて安価であるため、３軸センサを使用することは第１の実施形態に比べて経済的に有利である。
【００４４】
また、第１の実施形態の場合、複数（６つ）のリンク２４〜３４全てについて制振を実行している。しかしながら、実際には、全てのリンク２４〜３４に対して制振する必要はあまりない。図１を用いて説明すると、ロボット１０は、基本的には、手首部とアーム部とで構成される。アーム部のリンク２４〜２８は、ツール５０の位置と姿勢とを大きく制御するために、そのリンク長が手首部のリンク３０〜３４に比べて長い。一方、手首部のリンク３０〜３４は、ツール５０の位置と姿勢とを細かく制御するために、そのリンク長がアーム部のリンク２４〜２８に比べて短い。そのため、ロボット先端４８の振動に対する寄与は、リンク長が長いアーム部のリンク２４〜２８が振動する方が、リンク長が短い手首部のリンク３０〜３４が振動する方に比べて大きい。したがって、本実施形態は、リンク長が長いリンク２４〜２８に対して制振する、すなわちリンク２４〜２８を駆動するモータ３６〜４０に対して制振制御を実行する。
【００４５】
さらに、本実施形態では、上述したように３軸センサを使用するにあたり、またすべてのリンクではなくリンク長が長いアーム部のリンク２４〜２８に対して制振するにあたり、ヤコビ行列Ｊの部分行列を使用する。
【００４６】
具体的に説明すると、図４に示すように、ヤコビ行列Ｊは、３つの部分行列Ｊａ，Ｊｂ，Ｊｃから構成されている。部分行列Ｊａは、ヤコビ行列Ｊの第１行から第３行まで且つ第１列から第３列までの成分を抽出して構成される行列である。部分行列Ｊｂは、ヤコビ行列Ｊの第１行から第３行まで且つ第４列から第６列までの成分を抽出して構成される行列である。部分行列Ｊｃは、ヤコビ行列Ｊの第４行から第６行までの成分を抽出して構成される行列である。
【００４７】
ヤコビ行列Ｊを構成する３つの部分行列Ｊａ〜Ｊｃの中、３軸センサが検出したロボット先端の加速度
からリンク２４〜２８とモータ３６〜４０との間のねじれ角速度
を算出するために絶対に必要な行列は、部分行列Ｊａである。そこで、本実施形態は、第１の実施形態がヤコビ行列Ｊを使用しているのに対し、ヤコビ行列Ｊの部分行列Ｊａを使用する（以下、「部分ヤコビ行列Ｊａ」と称する）。６行６列のヤコビ行列Ｊに代って３行３列の部分ヤコビ行列Ｊａを使用するので、第１の実施形態に比べて、必要となる演算量を大幅に削減できる。
【００４８】
なお、発明者は、ヤコビ行列Ｊの代わりに部分ヤコビ行列Ｊａを使用するにあたり、実質的に問題がないことを確認している。
【００４９】
具体的には、図５にリンクモデルを示す６関節ロボットにおいて、６つのモータに同一の制御入力を与えたときの先端（フランジＦ）の基準座標系ΣＢにおける位置の変化量を、図５に示すヤコビ行列Ｊと部分ヤコビ行列Ｊａとを用いて算出した。制御入力を与える前の６つのモータの回転角度は、関節ＪＴ１のモータの回転角度θ１が２０°、関節ＪＴ２のモータの回転角度θ２が２０°、関節ＪＴ３のモータの回転角度θ３が３５°、関節ＪＴ４のモータの回転角度θ４は−４０°、関節ＪＴ５のモータの回転角度θ５は３０°、関節ＪＴ６のモータの回転角度θ６は−２０°である。この状態から各モータを１°回転させた。また、図に示す距離ｄ０は３８５ｍｍ、ｄ１は２１ｍｍ、ｄ２は１１００ｍｍ、ｄ３は１３００ｍｍ、ｄ４は０ｍｍ、ｄ５は２２８ｍｍである。さらに、図に示すように、関節ＪＴ１，ＪＴ２間のリンクＬ１、関節ＪＴ２，ＪＴ３間のリンクＬ２、および関節ＪＴ３，ＪＴ４間のリンクＬ３は、関節ＪＴ４，ＪＴ５間のリンク、関節ＪＴ５，ＪＴ６間のリンク，および関節ＪＴ６とフランジＦとの間のリンクに比べてそのリンク長が長い。
【００５０】
ヤコビ行列Ｊを用いて算出されたフランジＦの位置変化量（Δｘ１，Δｙ１，Δｚ１）は、（−２４．６，−１０．９，−２９．０）であった。一方、部分ヤコビ行列Ｊａを用いて算出されたフランジＦの位置変化量（Δｘ２，Δｙ２，Δｚ２）は、（−２６．６，−７．４２，−２７．１）であった。ヤコビ行列Ｊの位置変化量と部分ヤコビ行列Ｊａの位置変化量との誤差（ｘｅ，ｙｅ，ｚｅ）は、（２．０，−３．５，−１．９）である。この誤差をヤコビ行列Ｊの位置変化量に対する大きさで評価すると、５．５％（（ｘｅ２＋ｙｅ２＋ｚｅ２）／（Δｘ１２＋Δｙ１２＋Δｚ１２）×１００＝５．５）となり、軽微である。したがって、ヤコビ行列Ｊに代って部分ヤコビ行列Ｊａを用いても実質的には問題ない。
【００５１】
図６は、第２の実施形態のロボットの制御装置の一部、３つのモータ３６〜４０の制振制御に関連する構成要素のブロック図である。リンク、モータ、エンコーダ、およびモータ制御部は、第１の実施形態と同一である。
【００５２】
３軸センサ１５２は、基準座標系ΣＢのＸ，Ｙ，およびＺ軸方向の加速度
を検出する。
【００５３】
第１フィルタ１７０は、ロボット１０の停止直後に３軸センサ１５２が検出したロボット先端４８のＸ軸方向の加速度
、Ｙ軸方向の加速度
、Ｚ軸方向の加速度
に基づいて、ロボット先端４８のＸ軸方向の速度
、Ｙ軸方向の速度
、Ｚ軸方向の速度
を算出する。また、第１フィルタ１７０は、第１の実施形態の第１フィルタ７０と同様に、３軸センサ１５２の出力信号からＤＣ成分をカットする。
【００５４】
逆行列演算部１７２は、後述するねじれ角速度演算部１７４がねじれ角速度を算出するにあたって必要な部分ヤコビ行列Ｊａの逆行列Ｊａ−１算出する。
【００５５】
具体的には、３つのモータ３６〜４０の回転角度θ１〜θ３を変数とする関数の部分ヤコビ行列Ｊａ（図４参照）が予め定義されており、逆行列演算部１７２は、その部分ヤコビ行列Ｊａに回転角度θ１〜θ３を代入し、その後に部分ヤコビ行列Ｊａの逆行列Ｊａ−１を算出する。さらに具体的に言えば、逆行列演算部１７２は、３軸センサ１５２がロボット先端４８の加速度
を検出するタイミングにおける３つのモータ３６〜４０の回転角度θ１〜θ３に基づいて、部分ヤコビ行列Ｊａの逆行列Ｊａ−１を算出する。
【００５６】
ねじれ角速度演算部１７４は、リンク２４〜２８とモータ３６〜４０との間のねじれ角速度
を算出する。
【００５７】
具体的には、第１フィルタ１７０が算出したロボット先端４８の速度
と、逆行列演算部１７２が算出した部分ヤコビ行列Ｊａの逆行列Ｊａ−１とに基づいて、数式６を用いて、ねじれ角速度
を算出する。
【数６】
【００５８】
第２フィルタ１７６は、ねじれ角速度
に基づいて、リンク２４〜２８とモータ３６〜４０との間のねじれ角度α１〜α３を算出する。
【００５９】
具体的には、第２フィルタ１７６は、ねじれ角速度
を積分することによってねじれ角度α１〜α３を算出する。
【００６０】
補償量演算部１７８は、３つのモータ３６〜４０それぞれに対するモータ制御部８０の制御入力に加算する補償量Ｉ１〜Ｉ３を、第１の実施形態の補償量演算部７８と同様に算出する。
【００６１】
補償量演算部１７８が算出した補償量Ｉ１〜Ｉ３それぞれが対応するモータの制御入力に加算されることにより、３つのモータ３６〜４０それぞれが対応するリンクとの間のねじれが解消するように回転する。これにより、ロボット１０の動作の停止直後において、３つのリンク２４〜２８の振動の発生が抑制される。
【００６２】
本実施形態によれば、ロボット先端４８の加速度を検出するために３軸センサを使用するので、６軸センサを使用する第１の実施形態に比べて経済的である。また、６行６列のヤコビ行列Ｊの部分行列Ｊａを用いるので、第１の実施形態に比べて、必要となる演算量を大幅に削減できる。
【００６３】
以上、２つの実施形態を挙げて本発明を説明したが、本発明は上述の２つの実施形態に限定されない。
【００６４】
例えば、上述の実施形態の場合６関節のロボットであるが、本発明はこれに限らない。
【００６５】
また、上述の実施形態の場合、図３（図６）に示すように、各モータの回転角度θ１〜θ６（θ１〜θ３）をヤコビ行列Ｊ（部分ヤコビ行列Ｊａ）に代入してから逆行列Ｊ−１（Ｊａ−１）を算出している。この代わりとして、予めヤコビ行列Ｊの逆行列Ｊ−１を（部分ヤコビ行列Ｊａの逆行列Ｊａ−１）求めておき、その逆行列Ｊ−１に各モータの回転角度θ１〜θ６（θ１〜θ３）を代入するようにしてもよい。
【符号の説明】
【００６６】
２４〜３４ リンク
３６〜４６ モータ
５２ 加速度センサ（６軸センサ、３軸センサ）
【技術分野】
【０００１】
本発明は、ロボットの振動を抑制する制振方法およびロボットの制御装置に関する。
【背景技術】
【０００２】
従来より、先端に溶接ガンやスプレーガンなどのツールを取付けて動作する多関節ロボットは、動作の停止直後にその先端が振動するという問題がある。これは、ツールやロボットの先端部分の慣性により、ロボットのリンクが、片持ち梁のように振動することにより起こる。
【０００３】
このリンクの振動（先端の振動）に対処する方法として、例えば特許文献１に記載する方法がある。この特許文献１に記載する方法は、加速度センサをロボットの先端に取付け、その加速度センサが検出した先端の加速度に基づいてリンクを駆動するモータを制御することにより、リンクを制振し、ロボットの先端の振動を抑制する。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平４−１９３４９０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、上述の特許文献１の場合、加速度センサがロボットの先端の加速度を検出してから、その検出結果に基づいてリンクを制振するまでに大きな時間遅れが生じる。具体的に説明すると、加速度センサが検出したロボットの先端の加速度は、速度演算、座標演算を順に経てロボットの先端の位置変化量に演算される。この位置変化量に基づいてリンクを制振できる位置変更指令値が算出され、その位置変更指令値はリンクを駆動するモータに対する指令値に変換演算される。このように複数の演算を重ねるために、加速度センサがロボットの先端の加速度を検出してからモータの指令に反映されるまでの時間遅れが大きくなる。リンクの振動周期によっては（特に周期が短い場合）、この時間遅れによってリンクを十分に制振できず、最悪な場合、ロボットの先端の振動が長期化する可能性がある。
【０００６】
そこで、本発明は、ロボットの動作の停止直後において、加速度センサロボットの先端の加速度を検出してからすぐに、その加速度に基づいてリンクを駆動するモータを制御することにより、リンクを十分に制振し、先端の振動を抑制することを課題とする。
【課題を解決するための手段】
【０００７】
上述の課題を解決するために、本発明の請求項１に係る発明は、
複数のリンクと対応するリンクを駆動する複数のモータとを有するロボットの動作が停止するときに発生するロボット先端の振動を抑制するロボットの制振方法であって、
ロボット先端の加速度を加速度センサによって検出し、
加速度センサが検出した加速度に基づいてロボット先端の速度を算出し、
ロボット先端の速度と、加速度センサの加速度検出タイミングにおける複数のモータの回転角度と、ヤコビ行列の逆行列とに基づいて、各リンクと対応するモータとの間のねじれ角速度を算出し、
ねじれ角速度に基づいてねじれ角度を算出し、
ねじれ角速度とねじれ角度とに基づいて、各リンクと対応するモータとの間に発生したねじれが解消する、各モータの制御入力に加算する補償量を算出し、
補償量を加算した制御入力を各モータに出力することを特徴とする。
【０００８】
また、請求項２に係る発明は、
複数のリンクと対応するリンクを駆動する複数のモータとを有するロボットの動作が停止するときに発生するロボット先端の振動を抑制することが可能なロボットの制御装置であって、
ロボットの外部に予め定義されたＸＹＺ直交座標系と、
ロボット先端のＸ，Ｙ，およびＺ軸方向の加速度とＸ，Ｙ，およびＺ軸まわりの加速度とを検出する加速度センサと、
対応するモータの回転角度を検出する複数のエンコーダと、
加速度センサが検出した加速度に基づいてロボット先端の速度を算出する速度算出手段と、
速度算出手段が算出した速度と、加速度センサの加速度検出タイミングにおいて複数のエンコーダが検出した回転角度と、ヤコビ行列の逆行列とに基づいて、各リンクと対応するモータとの間のねじれ角速度を算出するねじれ角速度算出手段と、
ねじれ角速度算出手段が算出したねじれ角速度に基づいて、ねじれ角度を算出するねじれ角度算出手段と、
ねじれ角速度算出手段が算出したねじれ角速度と、ねじれ角度算出手段が算出したねじれ角度とに基づいて、各リンクと対応するモータとの間に発生したねじれが解消する、各モータの制御入力に加算する補償量を算出する補償量算出手段と、
補償量を加算した制御入力を各モータに出力するモータ制御手段とを有することを特徴とする。
【０００９】
さらに、請求項３に係る発明は、
３つのリンクと対応するリンクを駆動する３つのモータとを有するロボットの動作が停止するときに発生するロボット先端の振動を抑制することが可能なロボットの制御装置であって、
ロボットの外部に予め定義されたＸＹＺ直交座標系と、
ロボット先端のＸ，Ｙ，およびＺ軸方向の加速度を検出する加速度センサと、
対応するモータの回転角度を検出する複数のエンコーダと、
加速度センサが検出した加速度に基づいてロボット先端の速度を算出する速度算出手段と、
速度算出手段が算出した速度と、加速度センサの加速度検出タイミングにおいて複数のエンコーダが検出した回転角度と、ヤコビ行列の逆行列とに基づいて、各リンクと対応するモータとの間のねじれ角速度を算出するねじれ角速度算出手段と、
ねじれ角速度算出手段が算出したねじれ角速度に基づいてねじれ角度を算出するねじれ角度算出手段と、
ねじれ角速度算出手段が算出したねじれ角速度と、ねじれ角度算出手段が算出したねじれ角度とに基づいて、各リンクと対応するモータとの間に発生したねじれが解消する、各モータの制御入力に加算する補償量を算出する補償量算出手段と、
補償量を加算した制御入力を各モータに出力するモータ制御手段とを有することを特徴とする。
【００１０】
さらにまた、請求項４に係る発明は、
ロボットは、複数のリンクと対応するリンクを駆動する複数のモータとを有し、
上記３つのリンクは、複数のリンクの一部であって、他のリンクに比べて長いことを特徴とする。
【発明の効果】
【００１１】
本発明によれば、ロボットの動作の停止直後、加速度センサが検出した加速度からロボット先端の速度が算出され、その速度とヤコビ行列の逆行列とに基づいてリンクとモータとの間のねじれ角速度が算出される。次にねじれ角速度に基づいてねじれ角度が算出され、ねじれ角速度とねじれ角度に基づいて複数のモータそれぞれの制御入力に加算される、ねじれを解消する補償量が算出される。そして、補償量はそれぞれ、対応するモータの制御入力に加算される。
【００１２】
これにより、加速度センサが検出した加速度からモータの制御入力の補償量が短時間で算出され、その補償量が加算された制御入力によってモータが対応するリンクとの間のねじれを解消するためにすばやく回転する。したがって、加速度センサが検出した加速度からロボット先端の位置変化量を求め、位置変化量からリンクを制振できる位置姿勢変更指令値を算出し、その位置変更指令値をモータの指令値に演算してリンクを制振する場合に比べて、ロボット先端の加速度を検出してからリンクが制振されるまでの時間が短くなる。その結果、リンクを十分に制振でき、ロボット先端の振動を抑制することができる。
【図面の簡単な説明】
【００１３】
【図１】本発明の第１の実施形態に係るロボットのリンクモデルを示す図である。
【図２】リンクの振動を説明するための図である。
【図３】本発明の第１の実施形態に係るロボットの制御装置の一部のブロック図である。
【図４】ヤコビ行列の部分行列を説明するための図である。
【図５】ヤコビ行列の部分行列を説明するための別の図である。
【図６】第２の実施形態に係るロボットの制御装置の一部のブロック図である。
【発明を実施するための形態】
【００１４】
（第１の実施形態）
本発明の第１の実施形態に係るロボットのリンクモデルを図１に示す。図１に示すロボット１０は、６つの関節１２〜２２を備え、複数のリンク２４〜３４と対応するリンクを駆動する複数のモータ３６〜４６（図３参照）とを有する。モータ３６はリンク２４を駆動し、モータ３８はリンク２６、モータ４０はリンク２８、モータ４２はリンク３０、モータ４４はリンク３２、モータ４６はリンク３４を駆動する。
【００１５】
また、ロボット１０は、先端４８に溶接ガン、スプレーガン、またはハンドツールなどのツール５０を備えている。
【００１６】
さらに、ロボット１０は、その先端４８の加速度を検出する６軸センサ５２と、対応するモータの回転角度を検出する複数のエンコーダ５４〜６４（図３参照）とを備えている。６軸センサ５２を用いて、ロボットの外部に予め定義されている固定の基準座標系ΣＢを基準としてロボット先端４８の加速度を検出する。具体的には、例えば、６軸センサ５２のセンサ座標系に関する出力を、公知の手段によって基準座標系ΣＢのＸ，Ｙ，およびＺ軸方向の加速度と、Ｘ，Ｙ，およびＺ軸まわりの加速度に変換する。エンコーダ５４はモータ３６の回転角度θ１を検出し、エンコーダ５６はモータ３８の回転角度θ２、エンコーダ５８はモータ４０の回転角度θ３、エンコーダ６０はモータ４２の回転角度θ４、エンコーダ６２はモータ４４の回転角度θ５、エンコーダ６４はモータ４６の回転角度θ６を検出する。
【００１７】
さらにまた、ロボット１０（ロボット１０の制御装置）は、ロボット１０の動作の停止直後に、複数のリンク２４〜３４を制振するように構成されている。すなわち、ロボット１０の動作の停止直後に、複数のリンク２４〜３４を振動しないようにする。
【００１８】
まず、複数のリンク２４〜３４の制振を説明する前に、ロボット１０の動作の停止直後に発生する複数のリンク２４〜３４の振動（制振しない場合に発生する振動）について、図２を参照しながら説明する。
【００１９】
リンクの振動の発生は、主に３つのケースに分けられる。他のリンクの動作が停止することによってリンクが振動し始めるケース（図２（Ａ））と、モータによる駆動が停止することによってリンクが振動し始めるケース（図２（Ｂ））と、これら２つを組み合わせたケースがある。
【００２０】
例えば、図２（Ａ）に示すように、他のリンクが動作することによってリンクＬがそのツール側端が位置Ｐ０に位置する状態から移動し始め、そのツール側端が位置Ｐ１に到達したときに他のリンクが停止すると、リンクＬよりツール側に位置する他のリンク、モータ、ツールなどの慣性により、リンクＬがたわむ。このリンクＬのたわみにより、リンクＬのツール側端が位置Ｐ１を通過して位置Ｐ２に位置し、リンクＬと該リンクＬと連結されたモータＭとの間にねじれが発生する（ねじれ角度αが発生する）。そして、リンクＬは、このねじれを原因として、ツール側端が位置Ｐ１で停止するまで、振動し続ける。なお、本明細書では、「ねじれ角度α」は、図に示すように、ツール側端が本来あるべき位置Ｐ１とモータＭの中心とを結ぶ直線と、ツール側端の現状の位置Ｐ２とモータＭの中心とを結ぶ直線との間の角度を言う。
【００２１】
また例えば、図２（Ｂ）に示すように、モータＭに駆動されることによってリンクＬがそのツール側端が位置Ｐ０に位置する状態から回転し始め、角度β回転した後に（ツール側端が位置Ｐ１に到達したときに）モータＭが停止すると、リンクＬよりツール側に位置する他のリンク、モータ、ツールなどの慣性により、リンクがたわむ。このリンクのたわみにより、リンクのツール側端が位置Ｐ１を通過して位置Ｐ２に位置し、リンクＬと該リンクＬと連結されたモータＭとの間にねじれが発生する（ねじれ角度αが発生する）。そして、リンクＬは、このねじれを原因として、ツール側端が位置Ｐ１で停止するまで、振動し続ける。
【００２２】
このようなリンクとモータとの間に発生するねじれを原因として発生する振動を抑制するために、ロボット１０の制御装置は、複数のモータ３６〜４６に対して制振制御を実行する。
【００２３】
モータ３６〜４６に対して制振制御を実行するための構成を図３に示す。
【００２４】
図３は、ロボット１０の制御装置の一部、複数のモータ３６〜４６の制振制御に関連する構成要素のブロック図である。
【００２５】
ロボット１０の制御装置は、図３に示すように、ロボット先端４８の速度を算出する第１フィルタ７０と、ヤコビ行列の逆行列を算出する逆行列演算部７２と、リンク２４〜３４とモータ３６〜４６との間のねじれ角速度を算出するねじれ角速度演算部７４と、リンク２４〜３４とモータ３６〜４６との間のねじれ角度を算出する積分器７６と、モータ３６〜４６の制御入力に加算する補償量を算出する補償量演算部７８と、制御入力をモータ３６〜４６に出力するモータ制御部８０とを有する。
【００２６】
第１フィルタ７０は、ロボット１０の停止直後に６軸センサ５２が検出したロボット先端４８のＸ軸方向の加速度
、Ｙ軸方向の加速度
、Ｚ軸方向の加速度
、Ｘ軸まわりの加速度
、Ｙ軸まわりの加速度
、およびＺ軸まわりの加速度
に基づいて、ロボット先端４８のＸ軸方向の速度
、Ｙ軸方向の速度
、Ｚ軸方向の速度
、Ｘ軸まわりの速度
、Ｙ軸まわりの速度
、Ｚ軸まわりの速度
を算出する。具体的には、第１フィルタ７０は、６軸センサ５２から出力された信号からＨＰＦ（Ｈｉｇｈ Ｐａｓｓ Ｆｉｌｔｅｒ）によってＤＣ成分をカットし、その後に積分演算することにより、加速度
から速度
を算出する。
【００２７】
逆行列演算部７２は、後述するねじれ角速度演算部７４がねじれ角速度を算出するにあたって必要なヤコビ行列Ｊの逆行列をＪ−１算出する。
【００２８】
具体的には、複数のモータ３６〜４６の回転角度θ１〜θ６を変数とする関数のヤコビ行列Ｊ（数式１）が予め定義されており（例えば、制御装置の記憶部に記憶されており）、逆行列演算部７２は、そのヤコビ行列Ｊに回転角度θ１〜θ６を代入し、その後にヤコビ行列Ｊの逆行列Ｊ−１を算出する。さらに具体的に言えば、逆行列演算部７２は、６軸センサ５２がロボット先端４８の加速度
を検出するタイミングにおける複数のモータ３６〜４６の回転角度θ１〜θ６に基づいて、ヤコビ行列Ｊの逆行列Ｊ−１を算出する。なお、複数のモータ３６〜４６の回転角度θ１〜θ６は、対応するモータの回転角度を検出する複数のエンコーダ５４〜６４から得られる。
【数１】
【００２９】
ねじれ角速度演算部７４は、リンク２４〜３４とモータ３６〜４６との間のねじれ角速度
を算出する。なお、
はリンク２４とモータ３６との間のねじれ角速度であって、
はリンク２６とモータ３８、
はリンク２８とモータ４０、
はリンク３０とモータ４２、
はリンク３２とモータ４４、
はリンク３４とモータ４６との間のねじれ角速度である。
【００３０】
具体的には、第１フィルタ７０が算出したロボット先端４８の速度
と、逆行列演算部７２が算出したヤコビ行列Ｊの逆行列Ｊ−１とに基づいて、数式２を用いて、ねじれ角速度
を算出する。
【数２】
【００３１】
第２フィルタ７６は、ねじれ角速度
に基づいて、リンク２４〜３４とモータ３６〜４６との間のねじれ角度α１〜α６を算出する。
【００３２】
具体的には、第２フィルタ７６は、ねじれ角速度
を積分することによってねじれ角度α１〜α６を算出する。
【００３３】
補償量演算部７８は、複数のモータ３６〜４６それぞれに対するモータ制御部８０の制御入力（電流指令）に加算する補償量を算出する。
【００３４】
複数のモータ３６〜４６の補償量Ｉ１〜Ｉ６は、駆動するリンクのイナーシャ、バネ定数、およびダンパと、自身のトルク定数に基づいて算出される。
【００３５】
具体的に説明すると、リンクのイナーシャをＨ、バネ定数をＫ、ダンパをＤ、リンクとモータとの間のねじれ角度をα、ねじれ角速度をΔαとすると、リンクの運動方程式は、数式３のように表現することができる。
【数３】
【００３６】
数式３において、ａは、リンクの加速度である。
【００３７】
リンクの加速度ａを実現するために必要なモータ電流ＩＭは、数式４のように表現することできる。なお、数式４のＫtはモータのトルク定数である。
【数４】
【００３８】
数式４で表現されるモータ電流ＩＭは求めるべき補償量Ｉでもあるので、数式４を数式３に代入すると、数式５に示す補償量Ｉの式が得られる。
【数５】
【００３９】
補償量演算部７８が算出した補償量Ｉ１〜Ｉ６それぞれが対応するモータの制御入力に加算されることにより、複数のモータ３６〜４６それぞれが対応するリンクとの間のねじれが解消するように回転する。これにより、ロボット１０の動作の停止直後において、複数のリンク２４〜３４の振動の発生が抑制される。なお、このときロボット１０の動作は停止しているが、その停止姿勢を維持するために必要な制御入力が、モータ制御部８０から複数のモータ３６〜４６それぞれに出力されている。
【００４０】
本実施形態によれば、ロボット１０の動作の停止直後、６軸センサ５２が検出した加速度
からロボット先端４８の速度
が算出され、その速度
とヤコビ行列Ｊの逆行列Ｊ−１とに基づいてリンク２４〜３４とモータ３６〜４６との間のねじれ角速度
が算出される。次にねじれ角速度
に基づいてねじれ角度α１〜α６が算出され、ねじれ角速度
とねじれ角度α１〜α６に基づいて複数のモータ３６〜４６それぞれの制御入力に加算される、ねじれを解消する補償量Ｉ１〜Ｉ６が算出される。そして、補償量Ｉ１〜Ｉ６はそれぞれ、対応するモータの制御入力に加算される。
【００４１】
これにより、６軸センサ５２が検出した加速度
からモータ３６〜４６の制御入力の補償量Ｉ１〜Ｉ６が短時間で算出され、その補償量Ｉ１〜Ｉ６が加算された制御入力によってモータ３６〜４６が対応するリンク２４〜３４との間のねじれを解消するためにすばやく回転する。したがって、加速度センサが検出した加速度からロボット先端の位置変化量を求め、位置変化量からリンクを制振できる位置姿勢変更指令値を算出し、その位置変更指令値をモータの指令値に演算してリンクを制振する場合に比べて、ロボット先端の加速度を検出してからリンクが制振されるまでの時間が短くなる。その結果、リンクを十分に制振でき、ロボット先端の振動を抑制することができる。
【００４２】
（第２の実施形態）
本実施形態は、第１の実施形態に比べてより現実的且つ経済的な形態である。
【００４３】
第１の実施形態の場合、ロボット１０の停止直後において、複数のリンク２４〜３４を制振するために、６軸センサ５２を使用している。すなわち、ロボット先端のＸ軸方向の加速度
、Ｙ軸方向の加速度
、Ｚ軸方向の加速度
、Ｘ軸まわりの加速度
、Ｙ軸まわりの加速度
、およびＺ軸まわりの加速度
を検出している。しかしながら、実際に問題になるリンクやロボット先端の振動は、Ｘ軸方向、Ｙ軸方向、Ｚ軸方向、またはこれらを組み合わせた方向の振動であるため、ロボット先端の加速度としてＲｘ，Ｒｙ，Ｒｚ成分を検出する必要は現実的にはあまりない。したがって、本実施形態は、第１の実施形態と異なり、ロボット先端のＸ軸方向の加速度
、Ｙ軸方向の加速度
、Ｚ軸方向の加速度
を検出する３軸センサを使用する。３軸センサは６軸センサに比べて安価であるため、３軸センサを使用することは第１の実施形態に比べて経済的に有利である。
【００４４】
また、第１の実施形態の場合、複数（６つ）のリンク２４〜３４全てについて制振を実行している。しかしながら、実際には、全てのリンク２４〜３４に対して制振する必要はあまりない。図１を用いて説明すると、ロボット１０は、基本的には、手首部とアーム部とで構成される。アーム部のリンク２４〜２８は、ツール５０の位置と姿勢とを大きく制御するために、そのリンク長が手首部のリンク３０〜３４に比べて長い。一方、手首部のリンク３０〜３４は、ツール５０の位置と姿勢とを細かく制御するために、そのリンク長がアーム部のリンク２４〜２８に比べて短い。そのため、ロボット先端４８の振動に対する寄与は、リンク長が長いアーム部のリンク２４〜２８が振動する方が、リンク長が短い手首部のリンク３０〜３４が振動する方に比べて大きい。したがって、本実施形態は、リンク長が長いリンク２４〜２８に対して制振する、すなわちリンク２４〜２８を駆動するモータ３６〜４０に対して制振制御を実行する。
【００４５】
さらに、本実施形態では、上述したように３軸センサを使用するにあたり、またすべてのリンクではなくリンク長が長いアーム部のリンク２４〜２８に対して制振するにあたり、ヤコビ行列Ｊの部分行列を使用する。
【００４６】
具体的に説明すると、図４に示すように、ヤコビ行列Ｊは、３つの部分行列Ｊａ，Ｊｂ，Ｊｃから構成されている。部分行列Ｊａは、ヤコビ行列Ｊの第１行から第３行まで且つ第１列から第３列までの成分を抽出して構成される行列である。部分行列Ｊｂは、ヤコビ行列Ｊの第１行から第３行まで且つ第４列から第６列までの成分を抽出して構成される行列である。部分行列Ｊｃは、ヤコビ行列Ｊの第４行から第６行までの成分を抽出して構成される行列である。
【００４７】
ヤコビ行列Ｊを構成する３つの部分行列Ｊａ〜Ｊｃの中、３軸センサが検出したロボット先端の加速度
からリンク２４〜２８とモータ３６〜４０との間のねじれ角速度
を算出するために絶対に必要な行列は、部分行列Ｊａである。そこで、本実施形態は、第１の実施形態がヤコビ行列Ｊを使用しているのに対し、ヤコビ行列Ｊの部分行列Ｊａを使用する（以下、「部分ヤコビ行列Ｊａ」と称する）。６行６列のヤコビ行列Ｊに代って３行３列の部分ヤコビ行列Ｊａを使用するので、第１の実施形態に比べて、必要となる演算量を大幅に削減できる。
【００４８】
なお、発明者は、ヤコビ行列Ｊの代わりに部分ヤコビ行列Ｊａを使用するにあたり、実質的に問題がないことを確認している。
【００４９】
具体的には、図５にリンクモデルを示す６関節ロボットにおいて、６つのモータに同一の制御入力を与えたときの先端（フランジＦ）の基準座標系ΣＢにおける位置の変化量を、図５に示すヤコビ行列Ｊと部分ヤコビ行列Ｊａとを用いて算出した。制御入力を与える前の６つのモータの回転角度は、関節ＪＴ１のモータの回転角度θ１が２０°、関節ＪＴ２のモータの回転角度θ２が２０°、関節ＪＴ３のモータの回転角度θ３が３５°、関節ＪＴ４のモータの回転角度θ４は−４０°、関節ＪＴ５のモータの回転角度θ５は３０°、関節ＪＴ６のモータの回転角度θ６は−２０°である。この状態から各モータを１°回転させた。また、図に示す距離ｄ０は３８５ｍｍ、ｄ１は２１ｍｍ、ｄ２は１１００ｍｍ、ｄ３は１３００ｍｍ、ｄ４は０ｍｍ、ｄ５は２２８ｍｍである。さらに、図に示すように、関節ＪＴ１，ＪＴ２間のリンクＬ１、関節ＪＴ２，ＪＴ３間のリンクＬ２、および関節ＪＴ３，ＪＴ４間のリンクＬ３は、関節ＪＴ４，ＪＴ５間のリンク、関節ＪＴ５，ＪＴ６間のリンク，および関節ＪＴ６とフランジＦとの間のリンクに比べてそのリンク長が長い。
【００５０】
ヤコビ行列Ｊを用いて算出されたフランジＦの位置変化量（Δｘ１，Δｙ１，Δｚ１）は、（−２４．６，−１０．９，−２９．０）であった。一方、部分ヤコビ行列Ｊａを用いて算出されたフランジＦの位置変化量（Δｘ２，Δｙ２，Δｚ２）は、（−２６．６，−７．４２，−２７．１）であった。ヤコビ行列Ｊの位置変化量と部分ヤコビ行列Ｊａの位置変化量との誤差（ｘｅ，ｙｅ，ｚｅ）は、（２．０，−３．５，−１．９）である。この誤差をヤコビ行列Ｊの位置変化量に対する大きさで評価すると、５．５％（（ｘｅ２＋ｙｅ２＋ｚｅ２）／（Δｘ１２＋Δｙ１２＋Δｚ１２）×１００＝５．５）となり、軽微である。したがって、ヤコビ行列Ｊに代って部分ヤコビ行列Ｊａを用いても実質的には問題ない。
【００５１】
図６は、第２の実施形態のロボットの制御装置の一部、３つのモータ３６〜４０の制振制御に関連する構成要素のブロック図である。リンク、モータ、エンコーダ、およびモータ制御部は、第１の実施形態と同一である。
【００５２】
３軸センサ１５２は、基準座標系ΣＢのＸ，Ｙ，およびＺ軸方向の加速度
を検出する。
【００５３】
第１フィルタ１７０は、ロボット１０の停止直後に３軸センサ１５２が検出したロボット先端４８のＸ軸方向の加速度
、Ｙ軸方向の加速度
、Ｚ軸方向の加速度
に基づいて、ロボット先端４８のＸ軸方向の速度
、Ｙ軸方向の速度
、Ｚ軸方向の速度
を算出する。また、第１フィルタ１７０は、第１の実施形態の第１フィルタ７０と同様に、３軸センサ１５２の出力信号からＤＣ成分をカットする。
【００５４】
逆行列演算部１７２は、後述するねじれ角速度演算部１７４がねじれ角速度を算出するにあたって必要な部分ヤコビ行列Ｊａの逆行列Ｊａ−１算出する。
【００５５】
具体的には、３つのモータ３６〜４０の回転角度θ１〜θ３を変数とする関数の部分ヤコビ行列Ｊａ（図４参照）が予め定義されており、逆行列演算部１７２は、その部分ヤコビ行列Ｊａに回転角度θ１〜θ３を代入し、その後に部分ヤコビ行列Ｊａの逆行列Ｊａ−１を算出する。さらに具体的に言えば、逆行列演算部１７２は、３軸センサ１５２がロボット先端４８の加速度
を検出するタイミングにおける３つのモータ３６〜４０の回転角度θ１〜θ３に基づいて、部分ヤコビ行列Ｊａの逆行列Ｊａ−１を算出する。
【００５６】
ねじれ角速度演算部１７４は、リンク２４〜２８とモータ３６〜４０との間のねじれ角速度
を算出する。
【００５７】
具体的には、第１フィルタ１７０が算出したロボット先端４８の速度
と、逆行列演算部１７２が算出した部分ヤコビ行列Ｊａの逆行列Ｊａ−１とに基づいて、数式６を用いて、ねじれ角速度
を算出する。
【数６】
【００５８】
第２フィルタ１７６は、ねじれ角速度
に基づいて、リンク２４〜２８とモータ３６〜４０との間のねじれ角度α１〜α３を算出する。
【００５９】
具体的には、第２フィルタ１７６は、ねじれ角速度
を積分することによってねじれ角度α１〜α３を算出する。
【００６０】
補償量演算部１７８は、３つのモータ３６〜４０それぞれに対するモータ制御部８０の制御入力に加算する補償量Ｉ１〜Ｉ３を、第１の実施形態の補償量演算部７８と同様に算出する。
【００６１】
補償量演算部１７８が算出した補償量Ｉ１〜Ｉ３それぞれが対応するモータの制御入力に加算されることにより、３つのモータ３６〜４０それぞれが対応するリンクとの間のねじれが解消するように回転する。これにより、ロボット１０の動作の停止直後において、３つのリンク２４〜２８の振動の発生が抑制される。
【００６２】
本実施形態によれば、ロボット先端４８の加速度を検出するために３軸センサを使用するので、６軸センサを使用する第１の実施形態に比べて経済的である。また、６行６列のヤコビ行列Ｊの部分行列Ｊａを用いるので、第１の実施形態に比べて、必要となる演算量を大幅に削減できる。
【００６３】
以上、２つの実施形態を挙げて本発明を説明したが、本発明は上述の２つの実施形態に限定されない。
【００６４】
例えば、上述の実施形態の場合６関節のロボットであるが、本発明はこれに限らない。
【００６５】
また、上述の実施形態の場合、図３（図６）に示すように、各モータの回転角度θ１〜θ６（θ１〜θ３）をヤコビ行列Ｊ（部分ヤコビ行列Ｊａ）に代入してから逆行列Ｊ−１（Ｊａ−１）を算出している。この代わりとして、予めヤコビ行列Ｊの逆行列Ｊ−１を（部分ヤコビ行列Ｊａの逆行列Ｊａ−１）求めておき、その逆行列Ｊ−１に各モータの回転角度θ１〜θ６（θ１〜θ３）を代入するようにしてもよい。
【符号の説明】
【００６６】
２４〜３４ リンク
３６〜４６ モータ
５２ 加速度センサ（６軸センサ、３軸センサ）
【特許請求の範囲】
【請求項１】
複数のリンクと対応するリンクを駆動する複数のモータとを有するロボットの動作が停止するときに発生するロボット先端の振動を抑制するロボットの制振方法であって、
ロボット先端の加速度を加速度センサによって検出し、
加速度センサが検出した加速度に基づいてロボット先端の速度を算出し、
ロボット先端の速度と、加速度センサの加速度検出タイミングにおける複数のモータの回転角度と、ヤコビ行列の逆行列とに基づいて、各リンクと対応するモータとの間のねじれ角速度を算出し、
ねじれ角速度に基づいてねじれ角度を算出し、
ねじれ角速度とねじれ角度とに基づいて、各リンクと対応するモータとの間に発生したねじれが解消する、各モータの制御入力に加算する補償量を算出し、
補償量を加算した制御入力を各モータに出力することを特徴とするロボットの制振方法。
【請求項２】
複数のリンクと対応するリンクを駆動する複数のモータとを有するロボットの動作が停止するときに発生するロボット先端の振動を抑制することが可能なロボットの制御装置であって、
ロボットの外部に予め定義されたＸＹＺ直交座標系と、
ロボット先端のＸ，Ｙ，およびＺ軸方向の加速度とＸ，Ｙ，およびＺ軸まわりの加速度とを検出する加速度センサと、
対応するモータの回転角度を検出する複数のエンコーダと、
加速度センサが検出した加速度に基づいてロボット先端の速度を算出する速度算出手段と、
速度算出手段が算出した速度と、加速度センサの加速度検出タイミングにおいて複数のエンコーダが検出した回転角度と、ヤコビ行列の逆行列とに基づいて、各リンクと対応するモータとの間のねじれ角速度を算出するねじれ角速度算出手段と、
ねじれ角速度算出手段が算出したねじれ角速度に基づいて、ねじれ角度を算出するねじれ角度算出手段と、
ねじれ角速度算出手段が算出したねじれ角速度と、ねじれ角度算出手段が算出したねじれ角度とに基づいて、各リンクと対応するモータとの間に発生したねじれが解消する、各モータの制御入力に加算する補償量を算出する補償量算出手段と、
補償量を加算した制御入力を各モータに出力するモータ制御手段とを有することを特徴とするロボットの制御装置。
【請求項３】
３つのリンクと対応するリンクを駆動する３つのモータとを有するロボットの動作が停止するときに発生するロボット先端の振動を抑制することが可能なロボットの制御装置であって、
ロボットの外部に予め定義されたＸＹＺ直交座標系と、
ロボット先端のＸ，Ｙ，およびＺ軸方向の加速度を検出する加速度センサと、
対応するモータの回転角度を検出する複数のエンコーダと、
加速度センサが検出した加速度に基づいてロボット先端の速度を算出する速度算出手段と、
速度算出手段が算出した速度と、加速度センサの加速度検出タイミングにおいて複数のエンコーダが検出した回転角度と、ヤコビ行列の逆行列とに基づいて、各リンクと対応するモータとの間のねじれ角速度を算出するねじれ角速度算出手段と、
ねじれ角速度算出手段が算出したねじれ角速度に基づいてねじれ角度を算出するねじれ角度算出手段と、
ねじれ角速度算出手段が算出したねじれ角速度と、ねじれ角度算出手段が算出したねじれ角度とに基づいて、各リンクと対応するモータとの間に発生したねじれが解消する、各モータの制御入力に加算する補償量を算出する補償量算出手段と、
補償量を加算した制御入力を各モータに出力するモータ制御手段とを有することを特徴とするロボットの制御装置。
【請求項４】
ロボットは、複数のリンクと対応するリンクを駆動する複数のモータとを有し、
上記３つのリンクは、複数のリンクの一部であって、他のリンクに比べて長いことを特徴とする請求項３に記載のロボットの制御装置。
【請求項１】
複数のリンクと対応するリンクを駆動する複数のモータとを有するロボットの動作が停止するときに発生するロボット先端の振動を抑制するロボットの制振方法であって、
ロボット先端の加速度を加速度センサによって検出し、
加速度センサが検出した加速度に基づいてロボット先端の速度を算出し、
ロボット先端の速度と、加速度センサの加速度検出タイミングにおける複数のモータの回転角度と、ヤコビ行列の逆行列とに基づいて、各リンクと対応するモータとの間のねじれ角速度を算出し、
ねじれ角速度に基づいてねじれ角度を算出し、
ねじれ角速度とねじれ角度とに基づいて、各リンクと対応するモータとの間に発生したねじれが解消する、各モータの制御入力に加算する補償量を算出し、
補償量を加算した制御入力を各モータに出力することを特徴とするロボットの制振方法。
【請求項２】
複数のリンクと対応するリンクを駆動する複数のモータとを有するロボットの動作が停止するときに発生するロボット先端の振動を抑制することが可能なロボットの制御装置であって、
ロボットの外部に予め定義されたＸＹＺ直交座標系と、
ロボット先端のＸ，Ｙ，およびＺ軸方向の加速度とＸ，Ｙ，およびＺ軸まわりの加速度とを検出する加速度センサと、
対応するモータの回転角度を検出する複数のエンコーダと、
加速度センサが検出した加速度に基づいてロボット先端の速度を算出する速度算出手段と、
速度算出手段が算出した速度と、加速度センサの加速度検出タイミングにおいて複数のエンコーダが検出した回転角度と、ヤコビ行列の逆行列とに基づいて、各リンクと対応するモータとの間のねじれ角速度を算出するねじれ角速度算出手段と、
ねじれ角速度算出手段が算出したねじれ角速度に基づいて、ねじれ角度を算出するねじれ角度算出手段と、
ねじれ角速度算出手段が算出したねじれ角速度と、ねじれ角度算出手段が算出したねじれ角度とに基づいて、各リンクと対応するモータとの間に発生したねじれが解消する、各モータの制御入力に加算する補償量を算出する補償量算出手段と、
補償量を加算した制御入力を各モータに出力するモータ制御手段とを有することを特徴とするロボットの制御装置。
【請求項３】
３つのリンクと対応するリンクを駆動する３つのモータとを有するロボットの動作が停止するときに発生するロボット先端の振動を抑制することが可能なロボットの制御装置であって、
ロボットの外部に予め定義されたＸＹＺ直交座標系と、
ロボット先端のＸ，Ｙ，およびＺ軸方向の加速度を検出する加速度センサと、
対応するモータの回転角度を検出する複数のエンコーダと、
加速度センサが検出した加速度に基づいてロボット先端の速度を算出する速度算出手段と、
速度算出手段が算出した速度と、加速度センサの加速度検出タイミングにおいて複数のエンコーダが検出した回転角度と、ヤコビ行列の逆行列とに基づいて、各リンクと対応するモータとの間のねじれ角速度を算出するねじれ角速度算出手段と、
ねじれ角速度算出手段が算出したねじれ角速度に基づいてねじれ角度を算出するねじれ角度算出手段と、
ねじれ角速度算出手段が算出したねじれ角速度と、ねじれ角度算出手段が算出したねじれ角度とに基づいて、各リンクと対応するモータとの間に発生したねじれが解消する、各モータの制御入力に加算する補償量を算出する補償量算出手段と、
補償量を加算した制御入力を各モータに出力するモータ制御手段とを有することを特徴とするロボットの制御装置。
【請求項４】
ロボットは、複数のリンクと対応するリンクを駆動する複数のモータとを有し、
上記３つのリンクは、複数のリンクの一部であって、他のリンクに比べて長いことを特徴とする請求項３に記載のロボットの制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２０１１−１３６３９５（Ｐ２０１１−１３６３９５Ａ）
【公開日】平成２３年７月１４日（２０１１．７．１４）
【国際特許分類】
【出願番号】特願２００９−２９７６２０（Ｐ２００９−２９７６２０）
【出願日】平成２１年１２月２８日（２００９．１２．２８）
【出願人】（００００００９７４）川崎重工業株式会社 (1,710)
【Ｆターム（参考）】
【公開日】平成２３年７月１４日（２０１１．７．１４）
【国際特許分類】
【出願日】平成２１年１２月２８日（２００９．１２．２８）
【出願人】（００００００９７４）川崎重工業株式会社 (1,710)
【Ｆターム（参考）】
[ Back to top ]