(57)【要約】【課題】垂直移動軸を有するロボットについて、動作速度を向上させることができる軌道生成方法を提供する。【解決手段】シャフトを備えた水平４軸型のロボットを制御対象とする。コントローラは、手先を始点から終点に移動させる軌道を生成する際に、シャフトについては、その一端側に位置する手先が移動することに伴い、シャフトに作用する曲げモーメントＭ及びねじりモーメントＴから許容曲げモーメントＭｍａｘを求める（Ｓ２１）。そして、手先部分の質量，許容曲げモーメントＭｍａｘ，手先の垂直方向位置ｄに応じて手先の最大加速度Ａｍａｘを求め（Ｓ２２）、最大加速度Ａｍａｘを手先の軌道上における加速度制約λ″maxに変換して加速度条件を設定する（Ｓ２３）。【選択図】図１
2015058497.tif 000002
【特許請求の範囲】
【請求項１】
シャフトを垂直方向に移動させる垂直移動軸を有する水平多軸型ロボットについて、前記垂直移動軸の一端側に位置する手先を、始点から終点まで移動させるための当該手先の軌道を生成する方法において、
前記始点及び前記終点の位置を指定する位置指定ステップと、
各軸の駆動に関する拘束条件を加味して各軸の速度パターンを演算することで、移動時間が最短となるように前記軌道を生成する軌道生成ステップとを有し、
前記軌道生成ステップにおいて、前記拘束条件は、前記各軸について設定されている加速度条件を含み、
前記垂直移動軸については、前記シャフトの一端側に位置する手先が移動することに伴い、前記シャフトに作用する曲げモーメント及びねじりモーメントから許容曲げモーメントを求め、
前記手先部分の質量と、前記許容曲げモーメントと、前記手先の垂直方向位置とに応じて、前記手先の最大加速度を求めると、
前記最大加速度を、前記手先の軌道上における加速度制約に変換して、前記加速度条件を設定することを特徴とする水平多軸型ロボットの軌道生成方法。
【請求項２】
シャフトを垂直方向に移動させる垂直移動軸を有する水平多軸型ロボットについて、前記垂直移動軸の一端側に位置する手先を、始点から終点まで移動させるための当該手先の軌道を生成する制御装置において、
前記始点及び前記終点の位置を指定する位置指定手段と、
各軸の駆動に関する拘束条件を加味して各軸の速度パターンを演算することで、移動時間が最短となるように前記軌道を生成する軌道生成手段とを有し、
前記軌道生成手段は、前記拘束条件を、前記各軸について設定されている加速度条件とし、
前記垂直移動軸については、前記シャフトの一端側に位置する手先が移動することに伴い、前記シャフトに作用する曲げモーメント及びねじりモーメントから許容曲げモーメントを求め、
前記手先部分の質量と、前記許容曲げモーメントと、前記手先の垂直方向位置とに応じて、前記手先の最大加速度を求めると、
前記最大加速度を、前記手先の軌道上における加速度制約に変換して、前記加速度条件を設定することを特徴とする水平多軸型ロボットの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、シャフトを垂直方向に移動させる垂直移動軸を有する水平多軸型ロボットの軌道生成方法及び制御装置に関する。
【背景技術】
【０００２】
図１０は、水平４軸型のロボットの構成を概略的に示している（例えば特許文献１参照）。ロボット１１において、基部１２の上端には、第１軸をなす第１アーム１３の根元が接続されており、第１アーム１３の先端部には、第２軸をなす第２アーム１４の根元が接続されている。第２アーム１４の先端部には、第３軸をなし、垂直方向（上下方向，Ｚ軸方向）に移動するシャフト１５が配置されており、シャフト１５の下端側に、第４軸により回転するハンド１６（エンドエフェクタ）が配置されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１２−１９６７１７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
シャフト１５は、ハンド１６がワーク１７を把持した状態で上下方向に移動する。ハンド１６が基部２の接地面に近い位置にあると、第２アーム１４の先端部下面よりシャフト１５が下方に伸びる長さが長くなる。この場合、ハンド１６及びワーク１７の重量によりシャフト１５が撓み振動が発生し易い状態になることから、振動の発生を抑制するため、シャフト１５を上下動させる加速度には一定の制限がかけられている。つまり、シャフト１５の下端を最も下降させた状態で発生する振動が許容範囲内となるように制限されている。
しかしながら、実際には、ハンド１６のＺ軸位置に応じて発生する振動量は異なるはずである。それにも関わらず、第３軸の移動加速度が一定の上限値で制限されているため、ロボットの作業効率を向上させることができないという問題があった。
【０００５】
本発明は上記事情に鑑みてなされたものであり、その目的は、垂直移動軸を有するロボットについて、動作速度を向上させることができる水平多軸型ロボットの軌道生成方法及び制御装置を提供することにある。
【課題を解決するための手段】
【０００６】
請求項１記載の水平多軸型ロボットの軌道生成方法によれば、先ず位置指定ステップにおいて、手先の始点及び終点の位置を指定すると、軌道生成ステップでは、各軸の駆動に関する拘束条件を加味して各軸の速度パターンを演算することで、移動時間が最短となるように前記軌道を生成する。このとき、垂直移動軸については、シャフトの一端側に位置する手先が移動することに伴い、シャフトに作用する曲げモーメント及びねじりモーメントから許容曲げモーメントを求める。そして、手先部分の質量と、許容曲げモーメントと、手先の垂直方向位置とに応じて手先の最大加速度を求めると、その最大加速度を、手先の軌道上における加速度制約に変換して加速度条件を設定する。これにより、垂直移動軸についての加速度条件は、手先の垂直方向位置に応じて、シャフトに作用する許容曲げモーメントを満たす範囲で可変設定される。したがって、手先を垂直方向に移動させる時間を従来よりも短縮できるようになり、ロボットの作業効率を向上させることができる。
【図面の簡単な説明】
【０００７】
【図１】一実施形態を示すもので、ロボットのシャフトを垂直方向に移動させるための最大加速度を決定する処理を示すフローチャート
【図２】（１１）式で決まる最大加速度の一例を示す図
【図３】ロボットシステムの概略構成図
【図４】ロボットのシミュレーション条件を示す図
【図５】シミュレーション結果を、本実施形態の場合と従来技術の場合とについて示す図
【図６】手先のＺ軸位置に応じたＹ軸方向加速度を本実施形態の場合と従来技術の場合とについて示す図
【図７】２軸ロボットの物理的パラメータ（動力学パラメータ）をＸＹ平面上に示す図
【図８】コントローラが実行する、速度パターンを作成するためのフローチャート
【図９】図８の処理の一部として実行される、拘束条件を加味したボブロー法による速度パターンの作成の詳細を示すフローチャート
【図１０】従来技術を説明する図
【発明を実施するための形態】
【０００８】
以下、一実施形態について図１から図９を参照しながら説明する。図３は、一般的な産業用ロボットのシステム構成を示している。ロボットシステム１は、ロボット２と、ロボット２を制御するコントローラ３（制御装置，制御手段）と、コントローラ３に接続されたティーチングペンダント４とから構成されている。
【０００９】
ロボット２は、例えば４軸の水平多関節型ロボットとして構成されている。ロボット２は、設置面に固定されるベース５と、ベース５上にＺ軸（垂直軸）方向の軸心を持つ第１軸Ｊ１を中心に回転可能に連結された第１のアーム６と、第１のアーム６の先端部上にＺ軸方向の軸心を持つ第２軸Ｊ２を中心に回転可能に連結された第２のアーム７と、第２のアーム７の先端部に上下動可能で且つ回転可能に設けられたシャフト８とから構成されている。シャフト８を上下動させる際の軸が第３軸Ｊ３であり、回転させる際の軸が第４軸Ｊ４である。シャフト８の先端部（下端部）には、フランジ９が位置決めされて着脱可能に取り付けられる。
【００１０】
ベース５、第１のアーム６、第２のアーム７、シャフト８およびフランジ９は、ロボット２のアームとして機能する。アーム先端であるフランジ９（手先に相当）には、図示はしないが、例えばエアチャックやハンドなどのツールが取り付けられる。ロボット２に設けられる複数の軸（Ｊ１〜Ｊ４）はそれぞれに対応して設けられるモータ（図示せず）により駆動される。各モータの近傍には、それぞれの回転軸の回転角度（回転位置）を検出するための位置検出器（図示せず）が設けられている。
一般に、産業用のロボットは、予めティーチングなどを実施することにより作成される所定の動作プログラムに従って動作するようになっている。コントローラ３は、その動作プログラムに基づいてモータの駆動をフィードバック制御し、ロボット２のアームの動作制御を行う。
【００１１】
ティーチングペンダント４は、例えば使用者が携帯あるいは手に所持して操作可能な程度の大きさで、例えば薄型の略矩形箱状に形成されている。ティーチングペンダント４には、各種のキースイッチが設けられており、使用者は、それらキースイッチにより種々の入力操作を行う。ティーチングペンダント４は、ケーブルを経由してコントローラ３に接続され、通信インターフェイスを経由してコントローラ３との間で高速のデータ転送を実行するようになっており、キースイッチの操作により入力された操作信号等の情報はティーチングペンダント４からコントローラ３へ送信される。
【００１２】
ここで、手先の軌道を計算するためにボブロー（Bobrow）法を用い、各経由点を通過しながら手先を移動させるのに最速となる速度パターンを演算する。このボブロー法を用いた演算を行うに当たっては、実際のロボットの運動方程式が考慮されるため、各軸の動作速度やトルクの制限など、ロボットの拘束条件を満たした上で最速となる速度パターンが求められる。
【００１３】
次に、上述した多軸ロボットの軌道情報を生成方法及びその軌道情報を使用した多軸ロボットの制御装置の一例を詳述する。図７には、ロボット２の物理的パラメータ（動力学パラメータ）をＸＹ平面上に示している。尚、Ｙ軸は上記Ｘ軸に直交しており、図３に示す正面側を座標の正側としている。図中の各パラメータは以下の内容を示す。
ｍ１：第１のリンクの質量［ｋｇ］
ｍ２：第２のリンク及び負荷の総質量［ｋｇ］
ｒＧ１：第１軸からの第１のリンクの重心までの距離
ｒＧ２：第２軸からの第２のリンクの重心までの距離
ｌ１：第１のリンクの長さ［ｍ］
Ｉ１：第１のリンクのリンク重心回りのイナーシャ［ｋｇｍ２］
Ｉ２：第２のリンクのリンク重心回りのイナーシャ［ｋｇｍ２］
θ１：第１軸の関節角度［ｄｅｇ］
θ２：第２軸の関節角度［ｄｅｇ］
上述した多軸ロボット２は４軸であり、本実施形態に係る軌道生成方法及び制御法は、これらの４軸のうち、何れかの２軸以上の複数軸について実施できる。しかしながら、以下の説明では、軌道生成及び制御の原理を判り易く説明するため、第１及び第２軸のみを計算対象として説明する。
【００１４】
ここで、以下の説明で使用する物理量θ，λに関して、以下のような定義を与えておくことにする。
θ  ：ロボットの位置を示す、各軸の関節角［ｒａｄ］である。
θ´：ロボットの速度を示す、各軸の関節角速度［ｒａｄ／ｓ］である。
θ″：ロボットの加速度を示す、各軸の関節角加速度［ｒａｄ／ｓ２］である。
λ  ：軌道上の各位置を示す軌道パラメータである。
λ´：軌道上の各位置における傾きであり、軌道の速度を示す。
λ″：軌道上の各位置における曲率を示す、軌道の加速度を示す。
【００１５】
上述した多軸ロボット２の運動方程式の一般表記は（１）式となる。
【数１】
2015058497.tif 000003
但し、τはトルクベクトルである。そして、（１）式における慣性行列Ｈと、遠心力・コリオリ力を表す行列ｈとに、ロボット２の物理的パラメータが反映されている。ここで、
【数２】
2015058497.tif 000004
（２）式におけるＩ１１〜Ｉ２２，Ｃ１，Ｃ２は、図７中に示したパラメータである。また、（１）式におけるＢは摩擦，ｇは重力であるが、以下ではこれらを無視して説明する。
【００１６】
尚、ロボットの運動方程式の詳細については、以下の文献（１）を参照できる。
・＜文献（１）＞Proceedings of the 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems October 9-15,2006,Beijing China “Time-Optimal Trajectory Generation of Fast-Motion Planar Parallel Manipulator” by Yanjie Liu, Chenqi Wang, Juan Li, Lining Sun
【００１７】
図８は、図７に示す処理に対応してコントローラ３が実行するフローチャートである。先ず、コントローラ３は、例えばオペレータより与えられたロボット２の手先の始点，終点を読み込むと（Ｓ１，位置指定ステップ，位置指定手段）、その始点，終点の間の３点を、初期経由点（補間点）として決定する（Ｓ２，作成手段）。これらの初期経由点の決定には、例えば線形補間を用いる。例えば、始点座標が（０，０），終点座標が（３０，１００）であれば、初期経由点を（７．５，２５），（１５，５０），（２２．５，７５）とする。
【００１８】
次に、コントローラ３は、始点，終点及び上記３点の初期経由点の間を、例えばスプライン補間等により補間処理して（Ｓ３）、補間後の軌道についてボブロー法を用いて移動時間が最短となる速度パターンを作成する（Ｓ４，軌道生成ステップ）。尚、ボブロー法の詳細については、以下の，文献（２）（３）を参照。
・＜文献（２）＞The International Journal of Robotics Research,Vol.4, No.3,Fall 1985 p3-17“Time-Optimal Control of Robotic Manipulators Along Specified Paths” by J. E. Bobrow, S. Dubowsky, J. S. Gibson
・＜文献（３）＞IEEE JOURNAL OF ROBOTICS AND AUTOMATION,VOL.4,NO.4,AUGUST 1988 P443-450 “Optimal Robot Path Planning Using the Minimum-Time Criterion” by JAMES E. BOBROW
【００１９】
次に、ステップＳ４で行うボブロー法を用いた計算処理について、図９を参照して説明する。先ず、ステップＳ３で決定した手先の軌道を示す（３）式
ｘ＝ｆ（λ）            …（３）
より、軌道パラメータλを手先位置ｘ（ベクトル）に変換する（Ｓ１１）。尚、式に付している番号は、文献（１）に記載されているものについては文献（１）の番号に合わせている。軌道ｆ（λ）は、例えば３次スプラインで補間した場合は次式となる。
ｆ（λ）＝ａλ３＋ｂλ２＋ｃλ＋ｄ      …（３ａ）
但しλは、始点から終点までの座標の増分変数である。
【００２０】
続いて、逆運動学より、手先位置ｘを各軸（アーム）の角度ｑに変換する（Ｓ１２，変換手段）。
ｑ＝ｐ−１（ｆ（λ））      …（４）
但しｐは、順運動学を示す関数である。
そして、（１）式に（４）式を代入すると、（５）式が得られる（Ｓ１３）。
【数３】
2015058497.tif 000005
尚、Ｊはヤコビアンである。また、（５）式において摩擦Ｂ，重力ｇを夫々含むパラメータａ３，ａ４の項は「ゼロ」となる。
【００２１】
次に、ステップＳ１４において、コントローラ３は、（５）式より求められる最大加速度λ″max，最大減速度λ″minを計算する（これらについては、文献（１）の（８）〜（１０）式を参照）。尚、「λ」に付した「″」の記号は文献（１）において加速度を示すためのダブルドットに替わるものである。ここで求められる最大加速度λ″max，最大減速度λ″minが、ロボット２の動作の加速度に関する拘束条件となる。
【００２２】
ただし、ここで第３軸：シャフト８が対象である場合、最大加速度λ″max，最大減速度λ″minは（５）式によらず、図１に示す処理によって求める。先ず、ロボット２の機械的仕様に基づいて、許容曲げモーメントＭｍａｘを求める（Ｓ２１）。許容曲げモーメントＭｍａｘは、以下のように算出する。シャフト８の許容曲げ応力をσ［Ｎ／ｍｍ２］，断面係数をＺ［ｍｍ３］とすると、曲げモーメントＭ［Ｎ・ｍｍ］は次式となる。
Ｍ＝σ×Ｚ          …（６）
また、シャフト８の許容ねじり応力をＴａ［Ｎ／ｍｍ２］，極断面係数をＺｐ［ｍｍ３］とすると、ねじりモーメントＴ［Ｎ・ｍｍ］は次式となる。
Ｔ＝Ｔａ×Ｚｐ      …（７）
【００２３】
シャフト８に曲げモーメントＭと、ねじりモーメントＴとが加わるときの、相当曲げモーメントＭｅ［ｋｇｆ・ｍｍ］と、相当ねじりモーメントＴｅ［ｋｇｆ・ｍｍ］とを次式で計算する。
Ｍｅ＝｛Ｍ＋√（Ｍ２＋Ｔ２）｝／２      …（８）
Ｔｅ＝√（Ｍ２＋Ｔ２）                  …（９）
これらのうち最小値が、許容曲げモーメントＭｍａｘとなる。
Ｍｍａｘ＝Ｍｉｎ（Ｍｅ，Ｔｅ）        …（１０）
【００２４】
次に、ロボット２の手先のＺ軸位置ｄに応じて、手先の最大加速度Ａｍａｘを次式により求める（Ｓ２２）。尚、ｍは手先部分の質量であり、ハンドの質量，またハンドがワークを把持している場合はそのワークの質量も含む。
Ａｍａｘ＝Ｍｍａｘ／（ｍ×ｄ）    …（１１）
尚、図２には、上式で決まる最大加速度の一例を示す。
【００２５】
ここで、上記の最大加速度Ａｍａｘより、手先の軌道上における最大加速度λ″maxは、（１２）式で求められる（Ｓ２３）。
【数４】
2015058497.tif 000006
尚、（１２）式は、（３）式の左辺ｘをＡに置き換えて２階微分を行うことで導出される。ｆは上述のように軌道の関数であるから、ｆ´は軌道の傾きであり、ｆ″は軌道の曲率である。シャフト８に関しては、ステップＳ２３で求められた最大加速度λ″maxをステップＳ１４で使用する。
【００２６】
次いで、コントローラ３は、軌道上の次の位置λ及び速度λ´を、最大加速度λ″max(又は最大減速度λ″min)を用いて（１３）式，（１４）式により計算する（Ｓ１５）。ここで、時間ｔはサンプリングタイム（例えば１ｍｓ）であり、λ０，λ´０は現在の位置及び速度である。また「λ」に付した「´」は一階微分を表し、文献（１）において速度を示すためのドットに替わるものである。
λ＝０．５λ″maxｔ２＋λ´ｔ                …（１３）
λ´=λ″maxｔ                              …（１４）
【００２７】
次に、コントローラ３は、軌道パラメータλの速度λ´と加速度（減速度）λ″がそれぞれの拘束条件を満足するか否かを判定する（Ｓ１６）。つまり、コントローラ３は、
λ´min≦λ´≦λ´max                      …（１５）
λ″min(λ，λ´)≦λ″≦λ″max(λ，λ´)  …（１６）
の両条件を同時に満足するか否かを判定する。なお、加速時か減速時かに応じて、λ´min又はλ´max、及び、λ″min又はλ″maxが使用される。
【００２８】
また、軌道パラメータλの速度λ´に関する拘束条件の限界値λ´min、λ´maxは、
λ´min＝max(ｑ´min／ｆ´(λ))            …（１７）
λ´max＝min(ｑ´max／ｆ´(λ))            …（１８）
である。ここで、ｑ´min、ｑ´maxはロボット２の仕様より決まる最小及び最大の関節速度である。これらの速度条件及び加速度（トルク）条件は、拘束条件として必要な最小限の条件であり、状況に応じてこれ以外の条件を追加してもよい。
【００２９】
上述のステップＳ１６において、軌道パラメータλの速度λ´と加速度（減速度）λ″が式（１５）〜（１８）の拘束条件を共に満足していれば（ステップＳ１６、ＹＥＳ）、コントローラ３は、位置λをステップＳ１５で計算した値に更新する（Ｓ１７）。次いで、コントローラ３は、位置λが終点λendに到達したか否かを判断し（Ｓ１８）、到達していなければ（ＮＯ）処理をステップＳ１４に戻し、到達していれば（ＹＥＳ）処理を終了する。
【００３０】
一方、ステップＳ１６において、速度λ´と加速度（減速度）λ″が式（１３），（１４）の少なくも一方の拘束条件を満足していない場合（Ｓ１６，ＮＯ）、コントローラ３は、ステップＳ１９の処理を行う。すなわち、コントローラ３は、加速時であれば最大加速度λ″maxより任意の定数ａを減じたものを新たな最大加速度λ″maxとし、減速時であれば最大減速度λ″minより任意の定数ａを減じたものを新たな最大減速度λ″minとする。その後、ステップＳ１５に戻り、軌道パラメータである位置λ及び速度λ´を再計算する。
【００３１】
以上の図９に示す処理を完了すると、図８に示すステップＳ３で補間されて求められた軌道上を、ロボット２の手先が速度及び加速度（＝トルク）についての拘束条件を満たしつつ移動した場合に、移動速度が最も速くなる速度パターンが得られる。すなわち、ボブロー法を用いた計算処理はロボット２の運動力学に基づいて行われているので、ロボット２についての拘束条件を満たすことができる。
【００３２】
以上のように本実施形態によれば、垂直移動軸であるシャフト８を備えた水平４軸型のロボット２を制御対象とする。そして、コントローラ３は、手先を始点から移動させる軌道を生成する際に、垂直方向に移動するシャフト８については、シャフト８の一端側に位置する手先が移動することに伴い、シャフト８に作用する曲げモーメントＭ及びねじりモーメントＴから許容曲げモーメントＭｍａｘを求める。そして、手先部分の質量と、許容曲げモーメントＭｍａｘと、手先の垂直方向位置ｄとに応じて手先の最大加速度Ａｍａｘを求めると、その最大加速度Ａｍａｘを、手先の軌道上における加速度制約λ″maxに変換して加速度条件を設定する。これにより、垂直移動軸についての加速度条件は、手先の垂直方向位置に応じて、シャフト８に作用する許容曲げモーメントをＭｍａｘ満たす範囲で可変設定される。したがって、手先を垂直方向に移動させる時間を従来よりも短縮できるようになり、ロボット２の作業効率を向上させることができる。
【００３３】
本発明は上記した、又は図面に記載した各実施形態に限定されるものではなく、次のような変形または拡張が可能である。
４軸に限らず、少なくとも垂直移動軸（鉛直直動軸，Ｚ軸）を備え、前記垂直移動軸と共同して動作する多関節型のロボットであれば適用可能である。
【符号の説明】
【００３４】
図面中、２はロボット、３はコントローラ（制御装置，位置指定手段，軌道生成手段，）、６，７はアーム、８はシャフトを示す。
【図１】
2015058497.tif 000007
【図２】
2015058497.tif 000008
【図３】
2015058497.tif 000009
【図４】
2015058497.tif 000010
【図５】
2015058497.tif 000011
【図６】
2015058497.tif 000012
【図７】
2015058497.tif 000013
【図８】
2015058497.tif 000014
【図９】
2015058497.tif 000015
【図１０】
2015058497.tif 000016