JP7126222B2 - ロボット制御装置 - Google Patents
ロボット制御装置 Download PDFInfo
- Publication number
- JP7126222B2 JP7126222B2 JP2020518145A JP2020518145A JP7126222B2 JP 7126222 B2 JP7126222 B2 JP 7126222B2 JP 2020518145 A JP2020518145 A JP 2020518145A JP 2020518145 A JP2020518145 A JP 2020518145A JP 7126222 B2 JP7126222 B2 JP 7126222B2
- Authority
- JP
- Japan
- Prior art keywords
- acceleration
- torque
- joint
- time
- axis
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/416—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by control of velocity, acceleration or deceleration
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Human Computer Interaction (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Numerical Control (AREA)
- Manipulator (AREA)
Description
本発明は、ロボットの動作制御に係るロボット制御装置に関する。
モータによって回転駆動される関節軸を複数組備えた多関節型ロボットにおいて、ロボットアームを教示点間で高速移動させる場合、急激な加減速によって関節軸を駆動するモータに過大な負荷がかかることがある。
特許文献1には、各関節軸の最大または最小加速度到達時間を一定に保持したまま、動力学モデルを用いて求めたトルクが目標トルクになるように、基準関節軸の最大または最小加速度を修正する技術が開示されている。このようにすることで、加減速時のロボットアームの振動を抑えて、スムーズな加減速動作を行うことができる。
ところで、関節軸に発生するトルクは、当該関節軸に連結されたモータの出力トルクに依存し、当該モータの許容最大トルクによって制限される。すなわち、複数の関節軸が同時に動作する場合に、関節軸を動作させるのに必要なトルクが不足する場合がある。
例えば、加速時にトルクが不足したまま関節軸を回転させると、当該関節軸だけが遅れた動作となりアーム先端の軌道が予想外の軌道をたどる場合がある。すなわち、ロボットが周辺物と接触する恐れがある。また、減速時に必要なトルクが不足すると、目標教示点でオーバシュートする場合がある。その場合も、周辺物との接触の恐れがある。
通常、これらのような場合は、ロボットアームの動作プログラムの修正を行う。具体的には、加減速時間を調整する命令を加えたり、目標動作速度を変更したりして対応する。
しかし、この修正作業は、実際に動作プログラムを運転してカットアンドトライで行う必要があり、工数を要していた。
また、特許文献1に示す構成では、各関節軸の最大または最小加速度到達時間を一定に保持したまま、動力学モデルを用いて求めたトルクが目標トルクになるように、基準関節軸の最大または最小加速度を修正している。
しかし、この場合は、ロボットアームの動作は安定するものの、ロボットアームの動作時間であるタクトは増加してしまう。
本発明は、かかる点に鑑みてなされたもので、ロボットのタクトが増加するのを抑えつつ、関節軸を動作させるのに必要なトルクがモータの許容最大トルクを超えないように、加減速時間を自動調整する仕組みを備えたロボット制御装置を提供することにある。
上記目的を達成するため、本発明に係るロボット制御装置は、モータと該モータによって回転駆動される関節軸とを複数組有するロボットアームの動作を制御するロボット制御装置であって、各関節軸の回転速度と回転加速度と関節角度に基づいて、各関節軸における動力学トルクをそれぞれ算出して出力する動力学トルク算出部と、各関節軸の回転加速度が極大及び極小となる時点での各関節軸における関節角度の移動量と回転速度と前記回転加速度の極大値及び極小値とが各関節軸を加減速動作させるための加減速時間に関連付けられたテーブルを有する記憶部と、を少なくとも備え、前記動力学トルク算出部は、前記テーブル内の値に基づいて、前記回転加速度の極大値及び極小値に対応する動力学トルクを算出するとともに、前記回転加速度の極大値及び極小値に対応する慣性項トルクと、前記動力学トルクから前記慣性項トルクを差し引いた残余トルクとを算出し、各関節軸のうち少なくとも1つの関節軸において、前記慣性項トルクが各関節軸を所定の加速度で加減速動作させるに利用可能な利用可能トルクよりも大きい場合は、各関節軸の回転動作における共通の加減速時間を補正することを特徴とする。
この構成によれば、各関節軸の動作に必要なトルクの不足を予め検知でき、トルクの不足分に応じた値で加減速時間または移動時間を補正して、これらの値のいずれかを当初の値よりも大きくすることで、出力可能な範囲内のトルクで関節軸を回転駆動させて、ロボットアームの動作を制御することができる。このことにより、ロボットの動作プログラムの教示修正の手間を省くことができる。また、実際の加速度が極大または極小となる時刻、つまり、関節軸での加減速動作が最も急激に行われる時刻での動力学トルクに基づいて、加減速時間を補正するため、ロボットアームを安定して動作させつつ、ロボットアームの動作時間であるタクトが増加するのを抑えることができる。
以上説明したように、本発明に係るロボット制御装置によれば、ロボットアームを動作させるときの各関節軸で必要なトルクがその出力可能トルクを超えないように、加減速時間を自動調整することができ、ロボットの動作プログラムの教示修正の手間を省くことができる。また、ロボットのタクトが増加するのを抑制することができる。
以下、本発明の実施形態を図面に基づいて詳細に説明する。以下の好ましい実施形態の説明は、本質的に例示に過ぎず、本発明、その適用物或いはその用途を制限することを意図するものでは全くない。
(実施形態)
[ロボット制御装置の機能ブロック構成]
図1は、本実施形態に係るロボット制御装置の機能ブロック構成を示し、図2は、異なる関節軸であるi軸及びj軸における速度及び加速度の時間変化を示し、図3は、2軸ロボットの模式図を示す。なお、以降の説明において示す添字i,jは整数で1≦i,j≦nかつi≠jであり、nはロボットアーム6に含まれる関節軸の個数である。なお、以降の説明において、回転軸であるi軸やj軸の回転速度や回転加速度を単にi軸やj軸の速度や加速度と呼ぶことがある。
[ロボット制御装置の機能ブロック構成]
図1は、本実施形態に係るロボット制御装置の機能ブロック構成を示し、図2は、異なる関節軸であるi軸及びj軸における速度及び加速度の時間変化を示し、図3は、2軸ロボットの模式図を示す。なお、以降の説明において示す添字i,jは整数で1≦i,j≦nかつi≠jであり、nはロボットアーム6に含まれる関節軸の個数である。なお、以降の説明において、回転軸であるi軸やj軸の回転速度や回転加速度を単にi軸やj軸の速度や加速度と呼ぶことがある。
ロボット制御装置10は、動作プログラム1と、処理部2と、移動指令生成部3と、加減速処理部4と、サーボ5と、移動時間算出部7と、各軸速度算出部8と、加減速時間算出部9と、記録部20と、動力学トルク算出部21と、補正処理部22と、演算部23とを機能ブロックとして備えている。なお、図1に示す機能ブロックは、記録部20に保存された、または外部から読み込まれたソフトウェアをロボット制御装置10に設けられた図示しないCPU(Central Processing Unit)等の演算装置上で実行することで実現される。このソフトウェアに動作プログラム1が含まれていてもよい。また、説明の便宜上、ロボットアーム6に組み込まれた関節軸と、当該関節軸に連結されて関節軸を回転駆動するモータとで構成される複数の組については図示を省略している。
動作プログラム1には、ロボットアーム6の教示点位置と、教示点間を結ぶ軌跡の形状にあたる補間形態と、目標移動速度とが記録されている。教示点位置は、直交空間上の作業点位置またはロボットアーム6の関節角度である。目標移動速度は、教示点間を指示された補間形態で移動するときの目標速度である。
補間形態としては、直交空間上の作業点を、直線や円弧等の特定の形状を描くように補間するCP(continuas path)補間と、各関節軸を一定の速度で動かし、作業点の軌跡の形状は問わないPTP(point to point)補間とがあるが、以降は、PTP補間を行う場合について説明する。PTP補間は教示点間を最短時間で移動する場合に適用する補間形態であり、ロボットアーム6の関節軸のいずれかもしくは複数が、その最大速度で回転するように動作させることが多い。
処理部2は、動作プログラム1に記録されている教示点位置が直交空間上の値である場合に、それをロボットアーム6の各関節軸の関節角度に変換し、また、教示点位置がロボットアーム6の関節角度の場合は直交空間上の値に変換する。なお、以降の説明において、動作プログラム1上に記録された任意の教示点をPaとし、その次の教示点をPbとする。
移動指令生成部3は、隣り合う教示点間の移動量と目標移動速度とに基づいて移動指令を生成して出力する。この移動指令は単位時間当たりの関節角移動量(以下、単に角度移動量と呼ぶことがある。)である。各関節の角度移動量をまとめて1組とし、これを1つの移動指令とする。よって、移動指令はベクトル量である。移動指令生成部3は、後述する移動時間に対応する複数組の移動指令を時系列的に出力する。この時系列的に出力される複数組の移動指令を移動指令列と言う。なお、移動指令生成部3が出力する移動指令列には後述する加減速処理はなされていない。
加減速処理部4は、移動指令生成部3からの移動指令列に対して加減速処理を行って、加減速した移動指令を出力する。加減速処理部4は、後述する加減速時間算出部9が出力する加速時間で目標移動速度に達するように、移動指令生成部3からの移動指令列を加減速処理する。具体的には、図2に示すように、各関節軸における加速度を時間変化させて所定の目標移動速度に到達するようにする。また、教示点Pb手前で目標移動速度から減速を始めて、加減速時間算出部9が出力する減速時間が経過した後に教示点Pbに到達するように、移動指令生成部3からの移動指令列を減速処理する。なお、以降の説明において、加速度の時間変化波形を加速プロファイルまたは減速プロファイルあるいは加減速プロファイルと呼ぶことがある。なお、図2に示すように、加減速プロファイルは、ロボットアーム6の振動を誘発しないように時間に対して速度がS字状に変化する形状とする。なお、加減速処理は後述する演算部23が出力する補正後の加速時間または減速時間に基づいて行い、具体的な加減速処理の手順については後述する。
サーボ5は、加減速処理部4で処理された移動指令列に基づいて、各関節軸に連結されたモータを回転駆動させてロボットアーム6の動作を制御する。
移動時間算出部7は、ロボットアーム6の先端が教示点Paから教示点Pbへ加減速せずに目標移動速度で移動するのに要する所要時間を算出して出力する。当該所要時間が移動時間に相当する。
各軸速度算出部8は、PTP補間時の各関節軸の目標速度を算出し出力する。
加減速時間算出部9は、教示点Paから目標速度に加速するときの加速時間、教示点Pb手前で目標速度から減速して教示点Pbに到達するときの減速時間を求め出力する。
記憶部20は、後述するテーブル24(図7参照)や動作プログラム1を保存しており、ロボット制御装置10の各部からの要求に従って保存されたデータを対応するブロックに送信する。
動力学トルク算出部21は、テーブル24内のデータに基づいて、所定の時刻における各関節軸の動力学トルクをそれぞれ算出し出力する。
補正処理部22は、動力学トルク算出部21で算出された、所定の時刻における動力学トルク、慣性項トルク及び慣性項トルク以外のトルクに基づいて、後述する第1及び第2補正係数γa,γbを算出し出力する。
演算部23は、加減速時間算出部9で算出された加速時間Taまたは減速時間Tbに対して、補正処理部22で算出された第1または第2補正係数γa,γbを乗じて、補正後の加速時間Ta’または減速時間Tb’を求め、加減速処理部4に出力する。
テーブル24は、ロボットアーム6の各関節軸が駆動するときに、極大または極小となる実加速度及びそのときの速度と角度移動量が加減速時間に関連付けて配列されたテーブルである。
なお、加速時間は、例えば、教示点Paから加速を始めて目標移動速度に達するまでの時間をいい、減速時間とは、例えば、教示点Pbの手前で減速を始めて移動速度が零になるまでの時間をいい、加減速時間とは、これら両者を合わせた時間をいう。また、各関節軸について、各関節軸の許容最大加速度で目標速度まで加速したときの加速時間(各軸の最短加速時間と呼ぶことがある)を求め、これら各関節軸における最短加速時間の中で値が最大のものを選び、これを全軸共通の加速時間とする。図2に示すように、全ての関節軸はこの全軸共通の加速時間で加速する。また、減速時には、各関節軸における最短減速時間の中で値が最大のものを選び、これを全軸共通の減速時間とする。加速時と同様に、全ての関節軸はこの全軸共通の減速時間で減速する(図2参照)。
従って、テーブル24における加減速時間は全軸共通の加減速時間である。なお、テーブル24の構成及び作成手順については後述する。
[加減速時間の補正手順]
図4は、j軸における加速時の速度、加速度及び動力学トルクの時間変化を示す。なお、図4では、j軸の速度に相当する移動指令生成部3から出力された移動指令に基づいて算出された加速度やトルクを示している。また、図4において、実線は、移動指令に基づいて算出された動力学トルクを示し、破線は、実際にj軸に加わる動力学トルクを示す。
図4は、j軸における加速時の速度、加速度及び動力学トルクの時間変化を示す。なお、図4では、j軸の速度に相当する移動指令生成部3から出力された移動指令に基づいて算出された加速度やトルクを示している。また、図4において、実線は、移動指令に基づいて算出された動力学トルクを示し、破線は、実際にj軸に加わる動力学トルクを示す。
図4に示すように、動力学トルクは、加速度の上昇に伴い増加し、時刻t2で最大値になった後、加速度の低下に伴い減少する。ここで、j軸が出力可能な最大トルクをj軸に関する許容最大トルクτjmaxとすると、時刻t1からt3までの期間で、移動指令に基づいて算出された動力学トルクが許容最大トルクτjmaxを上回っている。これは、関節軸を駆動させるトルクが足らないことを意味する。このような場合には、例えば、加速時間を増加させる補正を行って、動力学トルクが許容最大トルクτjmax以下になるように調整する必要がある。
しかし、ロボットアーム6が接続された実際のロボット制御装置10では、サーボ制御系の遅れ等により、加減速時の移動指令よりも実際の速度の時間変化(以下、速度プロファイルと呼ぶことがある。)が小さくなることが多く(図4中の破線参照)、これに伴い、破線で示す実際の動力学トルクが指令速度に基づいて算出された動力学トルクよりも小さくなる。
よって、図4に示すように、実際には動力学トルクが許容最大トルクτjmaxを超えていないにも関わらず、加速時間を増加させる補正が必要と判断されてしまうことがある。この場合、タクトが増加するため、ロボットアーム6を用いた作業の生産性が低下するという問題があった。
そこで、本実施形態では、加速度が極大となるときの角度移動量や実速度を用いて動力学トルクを算出し、この動力学トルクに基づいて加減速時間の補正を行うことで、上記の不具合を解消している。
<STEP1:加減速時間の算出>
図5は、本実施形態に係る加減速時間の補正手順を示し、以降で説明する5つのステップを順次実行して加減速時間の補正を行う。
図5は、本実施形態に係る加減速時間の補正手順を示し、以降で説明する5つのステップを順次実行して加減速時間の補正を行う。
まず、対象となる系において、加速時間Ta及び減速時間Tbを算出する(STEP1)。
PTP補間の場合、教示点Pa、Pbでの関節角度をそれぞれθa、θbとし、教示点間に対応する作業点間の移動時間をTm、補間の単位時間をThとするとき、移動指令Δθは、式(1)で表わされる。なお、移動時間Tmは移動時間算出部7で算出される。
Δθ=(θb-θa)・Th/Tm ・・・(1)
ここで、θa、θb各関節軸の関節角度を要素とするベクトル量である。また、Δθは各関節軸の角度移動量を要素とするベクトル量である。
教示点Pa,Pbに対応する作業点間の移動距離をL、目標移動速度をVとするとき、移動時間Tmは、式(2a)で表わされる。
Tm=L/V ・・・(2a)
なお、PTP補間の場合、移動距離Lは、作業点間の直線距離に相当する。また、関節軸を加速または減速回転させてロボットアーム6を動作させる際に、自身の許容最大速度を超える関節軸がある場合は、当該関節軸の速度が許容最大速度に収まるように式(2a)から算出された移動時間Tmは修正される。
また、PTP補間において、教示点Pa,Pbに対応する作業点間を最短時間で移動する場合は、各関節軸の移動角度をその最大速度で除して得る値(当該軸の最短移動時間と呼ぶことがある)の中の最大値を移動時間Tmとする。
すなわち、複数の関節軸のうちj軸の移動角度をΔθj、最大速度をωjmaxとするとき、移動時間Tmは式(2b)で表わされる。
Tm=max{Δθj/ωjmax} ・・・(2b)
ここで、max{}は括弧内の要素(1個もしくは複数個)のうちの最大のものを選択することを意味する。
また、各軸速度算出部8で算出されたj軸に関する目標速度ωjcは、式(3)で表わされる。
ωjc=(θbj-θaj)/Tm ・・・(3)
ここで、θaj、θbjはそれぞれ、隣り合う教示点Pa、Pbでのj軸の関節角度であり、Tmは上述した移動時間である。よって、目標速度ωcは各関節軸の目標速度を要素とするベクトル量である。なお、ωjcはPTP補間前に予め値を求めることができる。
PTP補間の場合は、各軸速度算出部8からの各関節軸の目標速度に基づいて、最短の加減速時間を算出し出力する。
まず、j軸についての最短加速時間Tajを次のように求める。
Taj=|ωjc|/αajmax ・・・(4)
ここで、ωjcは各軸速度算出部8で算出されたj軸の目標速度、αajmaxは、教示点Paにおいてj軸だけを単独で加速する場合のj軸の許容最大加速度である。
教示点Paからロボットアーム6を移動させるときの加速時間Taは、式(5)で表わされる。
Ta=max{Taj} ・・・(5)
ここで、max{}は括弧内の複数の要素から値が最大のものを選択することを意味する。
j軸についての最短減速時間Tbjについても同様に式(6)で表わされる。
Tbj=|ωjc|/αbjmax ・・・(6)
ここで、αbjmaxは、教示点Pbにおいてj軸だけを単独で減速する場合のj軸の許容最大加速度である。
従って、教示点Pbへロボットアーム6を移動させるときの減速時間Tbは、式(7)で表わされる。
Tb=max{Tbj} ・・・(7)
<STEP2:テーブル作成>
次に、実際の加速度が極大となるときの角度移動量や速度を求め、これらに基づいてテーブル24を作成する(STEP2)。
次に、実際の加速度が極大となるときの角度移動量や速度を求め、これらに基づいてテーブル24を作成する(STEP2)。
図6Aは、加減速時間が100msecの場合の、図6Bは、加減速時間が150msecの場合の、図6Cは、加減速時間が200msecの場合の加速度、速度、角度移動量の時間変化をそれぞれ示す。図6A~6Cにおいて、移動指令及びそれに基づいた加速度を破線で示し、実際の値を実線で示している。なお、図6Aでは、100msecの間に移動指令を0から1まで増加させたときの加速度、速度、角度移動量を示している。また、100msecの間に移動指令を1から0まで減少させたときの角度移動量もあわせて図示している。同様に、図6Bでは、150msecの間に移動指令を0から1まで増加させたときの加速度、速度、角度移動量を、図6Cでは、200msecの間に移動指令を0から1まで増加させたときの加速度、速度、角度移動量をそれぞれ示している。
図6Aに示すように、加速中の回転軸の速度及び加速度は、移動指令及びそれに基づく加速度よりも小さくなっている。これは、前述したように、サーボ制御系の応答遅れ等に基づく。従って、実際の加減速プロファイルは台形形状とならずに、時刻taで極大値を有する形状となる。時刻taにおける実際の加速度α100,速度ω100、角度移動量Δθa100を加減速時間100msecに関連付けて配列する。また、図示しないが、100msecの間に移動指令を1から0まで減少させたときに、実際の加速度が極大となる時刻を時刻tb(図示せず)とし、時刻tbにおける関節角度θbまでの角度移動量Δθb100も加減速時間100msecに関連付けて加速度α100等と同様に配列する。
図6B,6Cに示すように、加減速時間を変化させて、それぞれ、加速度α100に対応する値α150,α200等を求めて、上記と同様に加減速時間に関連付けて配列する。このようにして図7に示すテーブル24を作成し、テーブル24は記憶部20に保存される。
なお、加速度α100,速度ω100、角度移動量Δθa100等は、実際のロボットアーム6の回転軸に各種センサを取付けて実測して求めてもよいし、他の方法で求めてもよい。例えば、ロボットアーム6とロボット制御装置10とで構成されるサーボ制御系のモデル及び各パラメータを用いて、図8に示すブロック線図に基づいて、シミュレーションによりこれらの値を導出するようにしてもよい。つまり、加減速時間を設定した後に、シミュレーションにより速度、加速度の時間変化を導出し、導出された加速度が極大となる値及びその時刻での速度、角度移動量を求めてテーブル24に組み込むようにする。なお、各関節軸の駆動に用いられる制御定数は、全軸共通であるため、テーブル24は1種類だけ準備される。ただし、6軸ロボット等では基本軸と手首軸とで異なる制御定数を用いる場合もある。このような場合は、図7には図示していないが、ロボットアーム6の関節軸のそれぞれについて加減速時間を共通とした上で、各データが取得されてテーブル24が作成される。また、テーブル24に含まれるデータの個数は、加減速時間の間隔により適宜決められる。
<STEP3:動力学トルク及び慣性項トルクの算出>
STEP2で作成したテーブル24に基づいて、動力学トルク算出部21で各関節軸の動力学トルク及び慣性項トルクをそれぞれ算出する(STEP3)。
STEP2で作成したテーブル24に基づいて、動力学トルク算出部21で各関節軸の動力学トルク及び慣性項トルクをそれぞれ算出する(STEP3)。
具体的には、テーブル24内の以下のパラメータ、加速度が極大となる時刻taにおける加速度αap、速度ωap、関節角度θap、加速度が極小となる時刻tbにおける加速度αbp、速度ωbp、関節角度θbp、また、ロボットアーム6のリンク長、重心位置、重心質量、リンクの慣性モーメント、モータのイナーシャ等の定数(図3参照)から、時刻ta及び時刻tbでの動力学トルクを算出する。
この動力学トルクは、関節角度θの場合に、速度ω及び加速度αで動作するのに必要な各関節軸のトルクであり、関節角度θ、速度ω、加速度αは、各関節軸の成分からなるベクトル量である。
また、時刻tにおける動力学トルクは式(15)で表わされる。
τ(t)=H(θ(t))・α(t)+D・ω(t)+b(ω(t))+c(θ(t)) ・・・(15)
ここで、H(θ(t))は関節角度θ(t)における慣性行列であり、Dは粘性係数からなる粘性行列である。さらに、b(ω(t))は、時刻tでの関節角度θ(t)における速度ω(t)による遠心力、コリオリ力の和、c(ω(t))は、関節角度θ(t)における重力トルクである。また、なお、式(15)は、ラグランジュ法またはニュートンオイラー法によりロボットアーム6の運動方程式を立てて解くことで導出できる。
ここで、式(15)の右辺を、第1項とそれ以外の部分とに分けて、式(16)で表わされる形式とする。なお、式(15)、(16)の右辺の第1項は加速度のみに依存する成分(以下、慣性項トルクという。)である。また、式(16)の第2項は速度ω(t)に依存する成分(以下、速度依存項トルクという。)、第2項は重力に依存する成分(以下、重力項トルクという。)である。なお、以降の説明において、τc(t)を残余トルクと呼ぶことがある。
τ(t)=τa(t)+τd(t)+τe(t)=τa(t)+τc(t) ・・・(16)
ただし、
τa(t)=H(θ(t))・α(t) ・・・(17)
τd(t)=D・ω(t)+b(ω(t)) ・・・(18)
τe(t)=c((ω(t)) ・・・(19)
τc(t)=τd(t)+τe(t) ・・・(20)
τa(t)=H(θ(t))・α(t) ・・・(17)
τd(t)=D・ω(t)+b(ω(t)) ・・・(18)
τe(t)=c((ω(t)) ・・・(19)
τc(t)=τd(t)+τe(t) ・・・(20)
動力学トルク算出部21では、上述した時刻taにおける加速度αap、速度ωap、関節角度θap、時刻tbにおける加速度αbp、速度ωbp、関節角度θbpを式(16)~(19)に代入して、時刻taでの慣性項トルクτap、速度依存項トルクτdap、重力項トルクτeap及び時刻tbでの慣性項トルクτbp、速度依存項トルクτdbp、重力項トルクτebpをそれぞれ算出する。
例えば、図3に示す2軸ロボットにおいて、第1軸の動力学トルクτ1(t)及び第2軸の動力学トルクτ2(t)は、それぞれ式(21)、(22)で表わされる。
τ1(t)=(H11・α1+H12・α2)+(D11・ω1+b1)+c1 ・・・(21)
τ2(t)=(H21・α1+H22・α2)+(D22・ω2+b2)+c2 ・・・(22)
τ2(t)=(H21・α1+H22・α2)+(D22・ω2+b2)+c2 ・・・(22)
なお、慣性行列H11~H22は以下の式(23)~(26)で表わされる。
H11=m1・lg12+m2・L12+m2・lg22+I1+I2+2・m2・L1・lg2・cosθ2+Jm1 ・・・(23)
H12=m2・lg22+I2+m2・L1・lg2・cosθ2 ・・・(24)
H21=m2・lg22+I2+m2・L1・lg2・cosθ2 ・・・(25)
H22=m2・lg22+I2+Jm2 ・・・(26)
H12=m2・lg22+I2+m2・L1・lg2・cosθ2 ・・・(24)
H21=m2・lg22+I2+m2・L1・lg2・cosθ2 ・・・(25)
H22=m2・lg22+I2+Jm2 ・・・(26)
また、粘性行列D11,D22は式(27)、(28)で表わされる。
D11=d1 ・・・(27)
D22=d2 ・・・(28)
D22=d2 ・・・(28)
また、速度依存項トルクであるb1,b2はそれぞれ式(29)、(30)で表わされる。
b1=-m2・L1・lg2・(2・ω1+ω2)・ω2・sinθ2 ・・・(29)
b2=m2・L1・lg2・ω12・sinθ2 ・・・(30)
b2=m2・L1・lg2・ω12・sinθ2 ・・・(30)
また、重力項トルクに対応するc1,c2はそれぞれ式(31)、(32)で表わされる。
c1=(m1・g・lg1+m2・g・L1)cosθ1+m2・g・lg2・cos(θ1+θ2) ・・・(31)
c2=m2・g・lg2・cos(θ1+θ2) ・・・(32)
c2=m2・g・lg2・cos(θ1+θ2) ・・・(32)
ここで、式(21)~(32)における各符号の内容は以下の通りである。
θ1:時刻tにおける第1軸の回転角(紙面における左回りを正方向とする。)
θ2:時刻tにおける第2軸の回転角(紙面における左回りを正方向とする。)
ω1:時刻tにおける第1軸の回転速度
ω2:時刻tにおける第2軸の回転速度
α1:時刻tにおける第1軸の回転加速度
α2:時刻tにおける第2軸の回転加速度
m1:第1軸のリンクの重心質量
m2:第2軸のリンクの重心質量、なお、m2にはリンク先端の負荷の質量が含まれる。
θ2:時刻tにおける第2軸の回転角(紙面における左回りを正方向とする。)
ω1:時刻tにおける第1軸の回転速度
ω2:時刻tにおける第2軸の回転速度
α1:時刻tにおける第1軸の回転加速度
α2:時刻tにおける第2軸の回転加速度
m1:第1軸のリンクの重心質量
m2:第2軸のリンクの重心質量、なお、m2にはリンク先端の負荷の質量が含まれる。
lg1:第1軸の回転中心から重心位置までの距離
lg2:第2軸の回転中心から重心位置までの距離
L1:第1軸の回転中心から第2軸の回転中心までの距離
I1:第1軸の、重心位置での回転軸回りの主慣性モーメント
I2:第2軸の、重心位置での回転軸回りの主慣性モーメント
Jm1:第1軸のモータイナーシャのアーム回転軸換算値
Jm2:第2軸のモータイナーシャのアーム回転軸換算値
d1:第1軸の粘性摩擦係数
d2:第2軸の粘性摩擦係数
g:重力加速度
である。
lg2:第2軸の回転中心から重心位置までの距離
L1:第1軸の回転中心から第2軸の回転中心までの距離
I1:第1軸の、重心位置での回転軸回りの主慣性モーメント
I2:第2軸の、重心位置での回転軸回りの主慣性モーメント
Jm1:第1軸のモータイナーシャのアーム回転軸換算値
Jm2:第2軸のモータイナーシャのアーム回転軸換算値
d1:第1軸の粘性摩擦係数
d2:第2軸の粘性摩擦係数
g:重力加速度
である。
<STEP4:補正係数の算出>
図9は、j軸における加減速時の関節角度、速度、加速度及び動力学トルクの時間変化を示す。また、図9は、補正係数γを導出するための考え方を示す模式図でもある。
図9は、j軸における加減速時の関節角度、速度、加速度及び動力学トルクの時間変化を示す。また、図9は、補正係数γを導出するための考え方を示す模式図でもある。
前述したように、補正処理部22は、動力学トルク算出部21で算出された、所定の時刻における動力学トルク、慣性項トルク及び慣性項トルク以外のトルク(速度依存項トルク、重力項トルク)に基づいて第1及び第2補正係数γa,γbを算出し出力する。
具体的な第1及び第2補正係数γa,γbの算出手順について以下に説明する。なお、j軸の出力可能な最大トルクを許容最大トルクτjmaxとする。また、本実施形態では、時刻ta,tbの前後における重力項トルクと速度依存項トルクの変化量は、慣性項トルクの変化量に比べて十分に小さいものとする。
まず、j軸に関する許容最大トルクτjmaxと、時刻taにおけるj軸の残余トルクτcapjとを用いて、式(33)に示すトルクτarjを算出する。
τarj=τjmax-τcapj ・・・(33)
なお、許容最大トルτjmaxの符号は時刻taにおける動力学トルクτj(ta)と同符号に合わせる。
次に、動力学トルク算出部21で算出された時刻taにおけるj軸の慣性項トルクτapjとトルクτarjを用いて、j軸に関する補正係数γajを算出する。
γaj=τapj/τarj ・・・(34)
同様に、j軸に関する許容最大トルクτjmaxと、時刻tbにおけるj軸の残余トルクτcbpjとを用いて、式(35)に示すトルクτbrjを算出する。
τbrj=τjmax-τcbpj ・・・(35)
なお、許容最大トルτjmaxの符号は時刻tbにおける動力学トルクτj(tb)と同符号に合わせる。
次に、動力学トルク算出部21で算出された時刻tbにおけるj軸の慣性項トルクτbpjとトルクτbrjを用いて、j軸に関する補正係数γbjを計算する。
γbj=τbpj/τbrj ・・・(36)
ここで、式(33)に示すトルクτarj及び式(35)中のトルクτbrjは、j軸における最大加速度での加減速回転動作を行う場合に、j軸の許容最大トルクτjmaxのうち利用可能なトルク(以下、利用可能トルクと呼ぶことがある。)に相当する。また、慣性項トルクτapj及びτbjは、j軸における最大加速度での加減速回転動作を実現するのに必要なトルクである。
従って、式(34),(36)に示すγaj,γbjが1以下であれば、j軸は所定の最大加速度で加減速回転動作が可能となる。一方、γaj,γbjが1よりも大きければ、j軸は所望の動作を実現するのに必要なトルクが足らないことを意味する。
次に、ロボットアーム6の全関節軸について、式(33)~(36)により補正係数をそれぞれ算出し、加速動作中に得られた補正係数の中の最大のものを選んで、第1補正係数γaとする。また、減速動作中に得られた補正係数の中の最大のものを選んで、第2補正係数γbとする。なお、以降の説明において、γa,γbを総称して補正係数γと呼ぶことがある。
γa=max{γaj} ・・・(37)
γb=max{γbj} ・・・(38)
γb=max{γbj} ・・・(38)
<STEP5:加減速時間の補正>
演算部23は、加減速時間算出部9で算出された加速時間Taに対して、補正処理部22で算出された第1補正係数γaを乗じて、補正後の加速時間Ta’を求め、加減速処理部4に出力する。また、演算部23は、加減速時間算出部9で算出された減速時間Tbに対して、補正処理部22で算出された第2補正係数γbを乗じて、補正後の減速時間Tb’を求め、加減速処理部4に出力する。なお、Ta’、Tb’はそれぞれ式(39)、(40)で表わされる。
演算部23は、加減速時間算出部9で算出された加速時間Taに対して、補正処理部22で算出された第1補正係数γaを乗じて、補正後の加速時間Ta’を求め、加減速処理部4に出力する。また、演算部23は、加減速時間算出部9で算出された減速時間Tbに対して、補正処理部22で算出された第2補正係数γbを乗じて、補正後の減速時間Tb’を求め、加減速処理部4に出力する。なお、Ta’、Tb’はそれぞれ式(39)、(40)で表わされる。
Ta’=Ta・γa ・・・(39)
Tb’=Tb・γb ・・・(40)
Tb’=Tb・γb ・・・(40)
式(33)~(40)より明らかなように、γa,γb≦1の場合は、いずれの関節軸においてもトルク不足は生じていないことを意味する。この場合は、第1及び第2補正係数γa,γbを乗じて得る補正後の加速時間Ta’及び減速時間Tb’は当初の加速時間Ta及び減速時間Tbと同じである。
一方、γa,γb>1の場合は、の場合は、いずれかの関節軸においてトルク不足が生じていることを意味する。そこで、当初の加速時間Taをγa(>1)倍した新たな加速時間Ta’(>Ta)で関節軸を回転させる。これは、関節軸の当初の加速度を1/γa倍に下げる、すなわち、慣性項トルクを1/γa倍に下げるということである。従って、第1補正係数γaにより補正されたj軸の動力学トルクτjは、その許容最大トルクτjmaxを超えることはない。同様に、第2補正係数γbにより補正されたj軸の動力学トルクτjは、その許容最大トルクτjmaxを超えることはない。
加減速処理部4は、この補正後の加速時間Ta’または減速時間Tb’を用いて、移動指令生成部3から出力された移動指令列に対して上述した加減速処理を行う。
[効果等]
以上説明したように、本実施形態におけるロボット制御装置10は、モータと該モータによって回転駆動される関節軸とを複数組有するロボットアーム6の動作を制御する。
以上説明したように、本実施形態におけるロボット制御装置10は、モータと該モータによって回転駆動される関節軸とを複数組有するロボットアーム6の動作を制御する。
また、ロボット制御装置10は、各関節軸の回転速度と回転加速度と関節角度に基づいて、各関節軸における動力学トルクをそれぞれ算出して出力する動力学トルク算出部21と、各関節軸の回転加速度が極大及び極小となる時点ta,tbでの各関節軸における関節角度の移動量Δθap,Δθbpと回転速度ωap,ωbpと回転加速度の極大値αap及び極小値αbpとが各関節軸を加減速動作させるための加減速時間Ta,Tbに関連付けられて保存されたテーブル24と、これが格納された記憶部20と、を少なくとも備えている。
動力学トルク算出部21は、テーブル24内の値に基づいて、回転加速度の極大値αap及び極小値αbpに対応する動力学トルクを算出するとともに、回転加速度の極大値αap及び極小値αbpに対応する慣性項トルクτap,τbpと、動力学トルクから慣性項トルクを差し引いた残余トルクτcap,τcbpとを算出し、各関節軸のうち少なくとも1つの関節軸において、慣性項トルクτapが各関節軸を所定の加速度で加減速動作させるに利用可能な利用可能トルク(τmax-τcap)よりも大きいか、慣性項トルクτbpが各関節軸を所定の加速度で加減速動作させるに利用可能な利用可能トルク(τmax-τcbp)よりも大きい場合は、各関節軸の回転動作における共通の加減速時間TaまたはTbを補正する。
本実施形態によれば、多関節ロボットにおいて、各関節軸での加減速に必要なトルクが不足しているかどうかを予め検知でき、トルクが不足している場合は、加速時間Taまたは減速時間Tbを補正して、出力可能な範囲内のトルクで関節軸の回転動作を加速して、ロボットアーム6の動作を制御することができる。このことにより、ロボットの動作プログラム1の教示修正の手間を省くことができる。
また、ロボット制御装置10は、ロボットアーム6における隣り合う作業点間距離Lと目標移動速度Vとに基づいて、各関節軸に対する移動指令Δθをそれぞれ生成して出力する移動指令生成部3と、一の関節軸であるj軸が出力可能な許容最大トルクτjmaxから残余トルクτcapまたはτcbpを差し引いた成分である利用可能トルクと、慣性項トルクτap,τbpとのトルク比を算出し、さらに各関節軸における当該トルク比をそれぞれ算出して、各関節軸におけるトルク比の中の最大値を補正係数γとして出力する補正処理部22と、を備えている。また、ロボット制御装置10は、補正係数γが1以下の場合は、予め設定された加速時間Taまたは減速時間Tbに基づいて移動指令Δθを加減速処理し、補正係数γが1を超える場合は、加速時間Taまたは減速時間Tbに補正係数γを乗じて補正された加速時間Ta’または減速時間Tb’に基づいて移動指令Δθを加減速処理する加減速処理部4と、を備えている。
加減速処理部4で加減速処理された移動指令Δθに基づいて各関節軸を回転駆動させてロボットアーム6の動作を制御する。
本実施形態によれば、多関節ロボットにおいて、各関節軸での加速に必要なトルクが不足しているかどうかを予め検知でき、トルクが不足している場合は、不足分に応じた値である第1補正係数γa(>1)で加速時間Taを補正して、加速時間Taを当初の値よりも大きな時間Ta’とすることで、出力可能な範囲内のトルクで関節軸の回転動作を加速して、ロボットアーム6の動作を制御することができる。このことにより、ロボットの動作プログラム1の教示修正の手間を省くことができる。同様に、トルクの不足分に応じた値である第2補正係数γb(>1)で減速時間Tbを補正して、減速時間Tbを当初の値よりも大きな時間Tb’とすることで、出力可能な範囲内のトルクで関節軸の回転動作を減速して、ロボットアーム6の動作を制御することができる。このことにより、ロボットの動作プログラム1の教示修正の手間を省くことができる。
また、実際の加速度が極大または極小となる時刻、つまり、関節軸での加減速動作が最も急激に行われる時刻での動力学トルクに基づいて、加速時間Ta及び/または減速時間Tbを補正するため、ロボットアーム6を安定して動作させつつ、ロボットアーム6の動作時間であるタクトが増加するのを抑えることができる。
テーブル24に保存された値は、移動指令に基づいて算出されるようにしてもよい。
このようにすることで、テーブル24の作成作業が簡素化されるとともに、作成時間を短縮することができる。
なお、本実施形態では、加減速プロファイルを台形形状としたが、これ以外のプロファイルに対しても適用できることは言うまでもない。
本発明のロボット制御装置は、タクトの増加を抑制しつつ、ロボットアームを移動させる際に各関節軸で必要なトルクがその出力可能トルクを超えないように、加減速時間を自動調整することができて、ロボットの動作プログラムの教示修正を少なくし、教示作業を効率的に行うことができて、複数軸からなるロボットに適用する上できわめて有用である。
1 動作プログラム
2 処理部
3 移動指令生成部
4 加減速処理部
5 サーボ
6 ロボットアーム
7 移動時間算出部
8 各軸速度算出部
9 加減速時間算出部
10 ロボット制御装置
20 記憶部
21 動力学トルク算出部
22 補正処理部
23 演算部
24 テーブル
2 処理部
3 移動指令生成部
4 加減速処理部
5 サーボ
6 ロボットアーム
7 移動時間算出部
8 各軸速度算出部
9 加減速時間算出部
10 ロボット制御装置
20 記憶部
21 動力学トルク算出部
22 補正処理部
23 演算部
24 テーブル
Claims (3)
- モータと該モータによって回転駆動される関節軸とを複数組有するロボットアームの動作を制御するロボット制御装置であって、
各関節軸の回転速度と回転加速度と関節角度に基づいて、各関節軸における動力学トルクをそれぞれ算出して出力する動力学トルク算出部と、
各関節軸の回転加速度が極大及び極小となる時点での各関節軸における関節角度の移動量と回転速度と前記回転加速度の極大値及び極小値とが各関節軸を加減速動作させるための加減速時間に関連付けられたテーブルを有する記憶部と、を少なくとも備え、
前記動力学トルク算出部は、前記テーブル内の値に基づいて、前記回転加速度の極大値及び極小値に対応する動力学トルクを算出するとともに、前記回転加速度の極大値及び極小値に対応する慣性項トルクと、前記動力学トルクから前記慣性項トルクを差し引いた残余トルクとを算出し、
各関節軸のうち少なくとも1つの関節軸において、前記慣性項トルクが各関節軸を所定の加速度で加減速動作させるに利用可能な利用可能トルクよりも大きい場合は、各関節軸の回転動作における共通の加減速時間を補正することを特徴とするロボット制御装置。 - 請求項1に記載のロボット制御装置において、
前記ロボットアームにおける隣り合う作業点間距離と目標移動速度とに基づいて、各関節軸に対する移動指令をそれぞれ生成して出力する移動指令生成部と、
一の前記関節軸が出力可能な許容最大トルクから前記残余トルクを差し引いた成分である前記利用可能トルクと、前記慣性項トルクとのトルク比を算出し、さらに各関節軸における当該トルク比をそれぞれ算出して、各関節軸における前記トルク比の中の最大値を補正係数として出力する補正処理部と、
前記補正係数が1以下の場合は、予め設定された前記加減速時間に基づいて前記移動指令を加減速処理し、前記補正係数が1を超える場合は、前記加減速時間に前記補正係数を乗じて補正された加減速時間に基づいて前記移動指令を加減速処理する加減速処理部と、をさらに備え、
前記加減速処理部で加減速処理された前記移動指令に基づいて各関節軸を回転駆動させて前記ロボットアームの動作を制御することを特徴とするロボット制御装置。 - 請求項1または2に記載のロボット制御装置において、
前記テーブルに保存された値は、移動指令に基づいて算出されることを特徴とするロボット制御装置。
Applications Claiming Priority (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2018091668 | 2018-05-10 | ||
JP2018091668 | 2018-05-10 | ||
PCT/JP2019/005302 WO2019215980A1 (ja) | 2018-05-10 | 2019-02-14 | ロボット制御装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
JPWO2019215980A1 JPWO2019215980A1 (ja) | 2021-05-20 |
JP7126222B2 true JP7126222B2 (ja) | 2022-08-26 |
Family
ID=68467986
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2020518145A Active JP7126222B2 (ja) | 2018-05-10 | 2019-02-14 | ロボット制御装置 |
Country Status (2)
Country | Link |
---|---|
JP (1) | JP7126222B2 (ja) |
WO (1) | WO2019215980A1 (ja) |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2007279899A (ja) | 2006-04-04 | 2007-10-25 | Mitsubishi Electric Corp | 数値制御装置 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0580833A (ja) * | 1991-09-18 | 1993-04-02 | Mitsubishi Electric Corp | 産業用ロボツトの制御装置 |
JP3359720B2 (ja) * | 1993-12-28 | 2002-12-24 | 三菱電機株式会社 | ロボット制御装置 |
JPH11259108A (ja) * | 1998-03-13 | 1999-09-24 | Mitsubishi Electric Corp | ロボット制御装置 |
JP2009028851A (ja) * | 2007-07-27 | 2009-02-12 | Nachi Fujikoshi Corp | ロボット制御装置 |
-
2019
- 2019-02-14 WO PCT/JP2019/005302 patent/WO2019215980A1/ja active Application Filing
- 2019-02-14 JP JP2020518145A patent/JP7126222B2/ja active Active
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2007279899A (ja) | 2006-04-04 | 2007-10-25 | Mitsubishi Electric Corp | 数値制御装置 |
Also Published As
Publication number | Publication date |
---|---|
WO2019215980A1 (ja) | 2019-11-14 |
JPWO2019215980A1 (ja) | 2021-05-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP3900789B2 (ja) | モータの速度・加速度決定方法、加減速生成方法、加減速制御方法、加減速制御装置及びモータ制御装置 | |
JP4281696B2 (ja) | 加減速制御方法及び装置並びに加減速制御方法のプログラム | |
JP7199178B2 (ja) | ロボット制御装置、ロボット装置、ロボット制御のパラメータ調整方法、およびプログラム | |
US9764471B2 (en) | Trajectory generation apparatus for robot to generate trajectory including curved portion | |
JP5916583B2 (ja) | 多関節ロボットのウィービング制御装置 | |
JP3981773B2 (ja) | ロボット制御装置 | |
JP7170181B2 (ja) | ロボット制御装置 | |
JP7126222B2 (ja) | ロボット制御装置 | |
JP2005052913A (ja) | ロボット制御装置 | |
JP5436930B2 (ja) | S字加減速軌道生成方法および多関節型ロボットシステム | |
CN112703090A (zh) | 机器人控制装置、机器人控制方法及机器人控制程序 | |
JP2016177513A (ja) | タンデム位置制御装置 | |
JP2016005296A (ja) | モータ駆動装置の制御方法、モータ駆動装置及びロボット装置 | |
JP5633268B2 (ja) | ロボットの制御装置 | |
JPH1039916A (ja) | 多関節ロボットの加減速制御方法 | |
JP6915470B2 (ja) | ロボットの制御装置 | |
JPH0276692A (ja) | ロボットの適応制御方法 | |
JP2012130999A (ja) | 制御装置およびロボットアームの制御方法 | |
JPH0546234A (ja) | ロボツトの加減速時定数決定方法 | |
JP4367041B2 (ja) | 機械制御装置 | |
JP6926882B2 (ja) | ロボットの制御装置 | |
JPH0760667A (ja) | ロボットのウィービング制御装置 | |
JPS63314607A (ja) | 多関節ロボットの制御装置 | |
JP2003076426A (ja) | 機械制御装置 | |
JPH0720940A (ja) | サーボ制御装置 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20210805 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20220802 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20220805 |
|
R151 | Written notification of patent or utility model registration |
Ref document number: 7126222 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |