JP4836872B2 - 制御装置、制御方法、プログラム及びロボット - Google Patents
制御装置、制御方法、プログラム及びロボット Download PDFInfo
- Publication number
- JP4836872B2 JP4836872B2 JP2007144999A JP2007144999A JP4836872B2 JP 4836872 B2 JP4836872 B2 JP 4836872B2 JP 2007144999 A JP2007144999 A JP 2007144999A JP 2007144999 A JP2007144999 A JP 2007144999A JP 4836872 B2 JP4836872 B2 JP 4836872B2
- Authority
- JP
- Japan
- Prior art keywords
- drive shaft
- output shaft
- compensation
- displacement amount
- drive
- 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
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
-
- 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/408—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 data handling or data format, e.g. reading, buffering or conversion of data
- G05B19/4086—Coordinate conversions; Other special calculations
-
- 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
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/41—Servomotor, servo controller till figures
- G05B2219/41357—Belt
-
- 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
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/41—Servomotor, servo controller till figures
- G05B2219/41413—Forward kinematics
-
- 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
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/42—Servomotor, servo controller kind till VSS
- G05B2219/42093—Position and current, torque control loop
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)
- Manipulator (AREA)
- Numerical Control (AREA)
Description
ΘJ=MTJΘM …(1)
目標位置算出部10、位置補償算出手段11、トルク補償算出手段17及び指令値算出部15は、モジュールとして中央演算処理装置(CPU)1に含まれる。目標位置算出部10が、データメモリ3からデータ列を読み出して、各制御周期における出力軸37の目標位置ΘJrを算出する。
ΔΘM=MTJΔΘJ …(2)
速度算出部16が、駆動軸位置検出器22により検出され、駆動軸位置入力手段101により入力された駆動軸位置の変位量ΘMに基づいて駆動軸速度を算出する。なお、速度算出部16が、出力軸位置の変位量ΘJに基づいて駆動軸速度を算出することもできるが、駆動軸36と出力軸37では減速比だけ速度が異なっており、同じ分解能の位置検出器を取り付けていれば、出力軸位置の変位量ΘJに基づくよりも、駆動軸位置の変位量ΘMに基づいて駆動軸速度を算出する方が、より高い精度での速度を算出することができるため、好ましい。
Θ'M=MTJΘJ …(3)
ここで、干渉行列MTJは、設計時の初期姿勢等のある姿勢(以下、「基準姿勢」という。)であって、外部から負荷が加わっていないときの幾何学的構造に基づいて予め算出したものを採用している。ロボットが基準姿勢より変化している場合、あるいは外部から負荷がかかっている場合においては、駆動力伝達手段38(ワイヤ)の伸びや経路長変化が生じ、駆動力伝達手段38において誤差が発生する。これにより干渉行列MTJが変動し、干渉行列MTJと異なる干渉行列MT'Jとなる場合がある。駆動軸位置検出器22により検出される実際の駆動軸位置の変位量ΘMは、干渉行列MT'Jを用いて式(4)のようになる。
ΘM=MT'JΘJ …(4)
このため、駆動軸位置検出器22により検出された駆動軸位置の変位量ΘMと、推定位置算出部18により算出した駆動軸推定位置Θ'Mは必ずしも一致せず、基準姿勢での干渉行列MTJからの変化量ΔMTJ(=MTJ−MT'J)に対応する駆動軸位置の差ΔΘM(=Θ'M−ΘM)が生じる場合がある。トルク補償算出部19が、干渉行列の変化量ΔMTJに相当する差ΔΘMを式(5)のように算出する。
ΔΘM=ΔMTJΘJ …(5)
更に、トルク補償算出部19が、差ΔΘMに基づいて駆動軸トルク補償τMを算出する。例えば、差ΔΘMを線形補償する場合、係数をKとして、式(6)のように算出される。
τM=KΔΘM …(6)
なお、線形補償する他にも、トルクオブザーバで実装する等してもよい。
(第1の変形例)
第1の変形例として、干渉行列メモリ4に記憶している干渉行列MTJを修正する場合を説明する。第1の変形例に係る制御装置は、図7に示すように、修正部41を更に備える点が、図1に示した制御装置の構成と異なる。
ΔΘM(tn)/dt=(Θ'M(tn)-ΘM(tn))-(Θ'M(tn-1)-ΘM(tn-1)) …(7)
ここで、干渉行列MTJは、出力軸位置ベクトルとの積を考慮すると、式(8)のように表現され、ΔΘMの成分ΔΘMiによって行ベクトルTMiが修正される。
MTJ=[TM1,TM2,…TMm]t …(8)
例えば、線形補償をする場合、ΔΘM(tn)を用いて、係数をKTiとして、式(9)のように干渉行列MTJの行ベクトルTMiが修正され、新しい干渉行列が得られる。
TMi=TMi+KTiΔΘMi(tn) …(9)
なお、ΔΘM(tn)/dtが大きいほど修正量が大きくなり、制御が不安定となる可能性が大きくなるので、ΔΘM(tn)/dtに対する閾値をデータメモリ3に記憶させておき、修正部41が、ΔΘM(tn)/dtを閾値と比較して、ΔΘM(tn)/dtが閾値よりも大きい場合には丸め処理等の補正を行う等しても良い。
(第2の変形例)
第2の変形例として、図10に示すように、力検出器(力センサ)42及び力補償算出部43を更に備える点が、図5に示した第1の変形例と異なる。
τJ=JtF …(10)
更に、力補償算出部43は、仮想仕事の原理より、MTJの逆行列である駆動軸−出力軸位置変換干渉行列JTMを用いて、式(11)のように出力軸トルクτJを各駆動軸36に負荷された駆動軸トルクτMに変換する。
τM=(MTt J)-1τJ=JTt MτJ …(11)
更に、力補償算出部43が、駆動軸トルクτMを用いて線形補償等の力補償を算出する。指令値算出部15は、出力軸位置補償及び駆動軸トルク補償に、力補償算出部43により算出した力補償を加算して指令値を算出する。他の構成は、図7に示した制御装置の構成と実質的に同様であるので、重複した説明を省略する。
(その他の実施の形態)
上記のように、本発明は実施の形態によって記載したが、この開示の一部をなす論述及び図面はこの発明を限定するものであると理解すべきではない。この開示から当業者には様々な代替実施の形態、実施例及び運用技術が明らかとなろう。
3…データメモリ
4…干渉行列メモリ(記憶手段)
10…目標位置算出部
11…位置補償算出手段
12…出力軸偏差算出部
13…駆動軸偏差算出部
14…位置補償算出部
15…指令値算出部
16…速度算出部
17…トルク補償算出手段
18…推定位置算出部
19…トルク補償算出部
21…駆動源
22…駆動軸位置検出器
23…出力軸位置検出器
30…本体
31,32…節(リンク)
33…駆動プーリ(関節)
34…従動プーリ(関節)
36…駆動軸(関節)
37…出力軸(関節)
38…駆動力伝達手段(ワイヤ)
39…減速機
41…修正部
42…力検出器
43…力補償算出部
101…駆動軸位置入力手段
102…出力軸位置入力手段
103…出力手段
104…力入力手段
Claims (12)
- 駆動軸、出力軸、前記駆動軸を駆動する駆動源、及び前記駆動軸から前記出力軸へ駆動力を伝達する駆動力伝達手段を有し、且つ前記駆動軸と前記出力軸の干渉を有する制御対象を制御する制御装置であって、
前記駆動軸の検出した位置の変位量を入力するための駆動軸位置入力手段と、
前記出力軸の検出した位置の変位量を入力するための出力軸位置入力手段と、
前記出力軸の位置と前記駆動軸の位置との関係を示す干渉行列に関するデータを保存するための記憶手段と、
前記出力軸の目標位置と入力された前記出力軸の位置の変位量に対して、前記データを用いて前記駆動軸に対する位置補償を算出する位置補償算出手段と、
前記入力された前記出力軸の位置の変位量及び入力された前記駆動軸の位置の変位量に対して、前記データを用いて前記駆動軸に対するトルク補償を算出するトルク補償算出手段と、
前記位置補償及び前記トルク補償から、前記駆動源への指令値を算出する指令値算出部と、
前記指令値に基づいて前記駆動軸を駆動するための信号を出力する出力手段
とを備えることを特徴とする制御装置。 - 前記位置補償算出手段が、
前記目標位置と前記入力された前記出力軸の位置の変位量との偏差を算出する出力軸偏差算出部と、
前記偏差に基づいて前記データを用いて駆動軸位置偏差を算出する駆動軸偏差算出部と、
前記入力された前記駆動軸の位置の変位量に基づいて駆動軸速度を算出する速度算出部と、
前記駆動軸位置偏差及び前記駆動軸速度から前記位置補償を算出する位置補償算出部
とを備えることを特徴とする請求項1に記載の制御装置。 - 前記トルク補償算出手段が、
前記入力された前記出力軸の位置の変位量を前記データを用いて駆動軸推定位置を算出する推定位置算出部と、
前記駆動軸推定位置と前記入力された前記駆動軸の位置の変位量との差から前記トルク補償を算出するトルク補償算出部
とを備えることを特徴とする請求項1又は2に記載の制御装置。 - 前記差に基づいて前記データを修正する修正部を更に備えることを特徴とする請求項3に記載の制御装置。
- 前記修正部が、
前記差の時間差分を算出し、
前記時間差分に基づいて、前記データを修正する
ことを特徴とする請求項4に記載の制御装置。 - 前記目標位置を算出するための目標位置算出部をさらに有し、
前記修正部が、前記目標位置を算出する周期よりも長い周期で前記データを修正することを特徴とする請求項3又は4に記載の制御装置。 - 前記出力軸へ付加される力を入力するための力入力手段と、
前記力に基づいて力補償を算出する力補償算出部
とを更に備えることを特徴とする請求項1〜6のいずれか1項に記載の制御装置。 - 前記出力軸へ付加されるトルクを入力するためのトルク入力手段と、
前記トルクに基づいて、前記データを用いて力補償を算出する力補償算出部 とを更に備えることを特徴とする請求項1〜6のいずれか1項に記載の制御装置。 - 前記指令値算出部が、前記力補償に基づいて前記指令値を算出することを特徴とする請求項7又は8に記載の制御装置。
- 駆動軸、出力軸、前記駆動軸を駆動する駆動源、及び前記駆動軸から前記出力軸へ駆動力を伝達する駆動力伝達手段を有し、且つ前記駆動軸と前記出力軸の干渉を有する制御対象を制御する制御方法であって、
前記駆動軸の検出した位置の変位量を入力するステップと、
前記出力軸の検出した位置の変位量を入力するステップと、
前記出力軸の目標位置と入力された前記出力軸の位置の変位量に対して、前記出力軸の位置と前記駆動軸の位置との関係を示す干渉行列に関するデータを用いて前記駆動軸に対する位置補償を算出するステップと、
前記入力された前記出力軸の位置の変位量及び入力された前記駆動軸の位置の変位量に対して、前記データを用いて前記駆動軸に対するトルク補償を算出するステップと、
前記位置補償及び前記トルク補償から、前記駆動源への指令値を算出するステップと、
前記指令値に基づいて前記駆動軸を駆動するための信号を出力するステップ
とを含むことを特徴とする制御方法。 - 駆動軸、出力軸、前記駆動軸を駆動する駆動源、及び前記駆動軸から前記出力軸へ駆動力を伝達する駆動力伝達手段を有し、且つ前記駆動軸と前記出力軸の干渉を有する制御対象を制御するプログラムであって、
前記出力軸の目標位置と入力された前記出力軸の位置の変位量に対して、前記出力軸の位置と前記駆動軸の位置との関係を示す干渉行列に関するデータを用いて前記駆動軸に対する位置補償を算出させる命令と、
前記入力された前記出力軸の位置の変位量及び入力された前記駆動軸の位置の変位量に対して、前記データを用いて前記駆動軸に対するトルク補償を算出させる命令と、
前記位置補償及び前記トルク補償から、前記駆動源への指令値を算出させる命令と、
前記指令値に基づいて前記駆動軸を駆動するための信号を出力させる命令
とを制御装置に実行させることを特徴とするプログラム。 - 請求項1〜9に記載の制御装置と、
前記駆動軸の位置の変位量を検出するための駆動軸位置検出器と、
前記出力軸の位置の変位量を検出するための出力軸位置検出器と、
前記出力手段から出力される信号に基づいて制御される駆動源
とを備えることを特徴とするロボット。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2007144999A JP4836872B2 (ja) | 2007-05-31 | 2007-05-31 | 制御装置、制御方法、プログラム及びロボット |
US12/129,749 US7847503B2 (en) | 2007-05-31 | 2008-05-30 | Control apparatus and method for controlling a control subject |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2007144999A JP4836872B2 (ja) | 2007-05-31 | 2007-05-31 | 制御装置、制御方法、プログラム及びロボット |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2008299590A JP2008299590A (ja) | 2008-12-11 |
JP4836872B2 true JP4836872B2 (ja) | 2011-12-14 |
Family
ID=40087375
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2007144999A Active JP4836872B2 (ja) | 2007-05-31 | 2007-05-31 | 制御装置、制御方法、プログラム及びロボット |
Country Status (2)
Country | Link |
---|---|
US (1) | US7847503B2 (ja) |
JP (1) | JP4836872B2 (ja) |
Families Citing this family (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8368152B2 (en) * | 2011-04-18 | 2013-02-05 | Taiwan Semiconductor Manufacturing Company, Ltd. | MEMS device etch stop |
WO2012153629A1 (ja) * | 2011-05-12 | 2012-11-15 | 株式会社Ihi | 運動予測制御装置と方法 |
JP5409844B2 (ja) * | 2012-05-30 | 2014-02-05 | 株式会社神戸製鋼所 | 多関節ロボットの軌跡制御装置および制御方法 |
JP5919143B2 (ja) * | 2012-08-31 | 2016-05-18 | 本田技研工業株式会社 | 駆動装置 |
JP5667147B2 (ja) * | 2012-10-30 | 2015-02-12 | ファナック株式会社 | ボールねじの伸縮量に基づいて補正処理を実行するサーボ制御装置 |
JP6496167B2 (ja) * | 2015-03-19 | 2019-04-03 | オークマ株式会社 | タンデム位置制御装置 |
CN108673495B (zh) * | 2018-04-13 | 2021-05-07 | 南京航空航天大学 | 一种力/位协调的多臂机器人柔顺控制方法 |
WO2021149563A1 (ja) * | 2020-01-20 | 2021-07-29 | ファナック株式会社 | ロボットシミュレーション装置 |
CN112077846B (zh) * | 2020-09-08 | 2022-02-18 | 西华大学 | 非线性机器人抗外源干扰控制器设计方法 |
CN113183154B (zh) * | 2021-05-10 | 2022-04-26 | 浙江工业大学 | 一种柔性关节机械臂的自适应反演控制方法 |
Family Cites Families (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2708458B2 (ja) * | 1988-04-01 | 1998-02-04 | 株式会社豊田中央研究所 | 倣い制御ロボット |
US5373747A (en) * | 1991-03-30 | 1994-12-20 | Kabushiki Kaisha Toshiba | Robot hand and robot |
JPH05164648A (ja) | 1991-12-18 | 1993-06-29 | Harmonic Drive Syst Ind Co Ltd | トルクセンサ |
KR100439466B1 (ko) * | 1995-09-11 | 2004-09-18 | 가부시키가이샤 야스가와덴끼 | 로봇제어장치 |
JPH11262841A (ja) * | 1998-03-19 | 1999-09-28 | Teijin Seiki Co Ltd | 回転テーブルの駆動・位置決め装置 |
JP2004171333A (ja) | 2002-11-21 | 2004-06-17 | Fuji Electric Fa Components & Systems Co Ltd | 機械制御装置 |
JP2006055927A (ja) * | 2004-08-18 | 2006-03-02 | Sharp Corp | 関節駆動装置および関節駆動装置の制御方法 |
CN101870112B (zh) * | 2006-01-13 | 2011-09-28 | 松下电器产业株式会社 | 机械手臂的控制装置 |
JP2008073830A (ja) * | 2006-09-25 | 2008-04-03 | Fanuc Ltd | ロボット制御装置 |
WO2009004772A1 (ja) * | 2007-07-05 | 2009-01-08 | Panasonic Corporation | ロボットアームの制御装置及び制御方法、ロボット、及び制御プログラム |
-
2007
- 2007-05-31 JP JP2007144999A patent/JP4836872B2/ja active Active
-
2008
- 2008-05-30 US US12/129,749 patent/US7847503B2/en active Active
Also Published As
Publication number | Publication date |
---|---|
US20080297094A1 (en) | 2008-12-04 |
US7847503B2 (en) | 2010-12-07 |
JP2008299590A (ja) | 2008-12-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP4836872B2 (ja) | 制御装置、制御方法、プログラム及びロボット | |
US8428779B2 (en) | Robot controlling device | |
US8155790B2 (en) | Robot control apparatus | |
JP6754771B2 (ja) | エンドエフェクタ位置推定を実行するロボット適応型配置システム | |
JP5370392B2 (ja) | ロボットシステム、ロボット制御装置およびロボット制御方法 | |
US9205560B1 (en) | System and method for failure detection of a robot actuator | |
US11148282B2 (en) | Control device for motor drive device, control device for multi-axial motor, and control method for motor drive device | |
US4989161A (en) | Control unit for a multi-degree-of freedom manipulator | |
US9874865B2 (en) | Control method and control device | |
JP5730614B2 (ja) | トルクセンサ校正装置、校正方法、及びプログラム | |
JPS63314606A (ja) | 多関節ロボットの制御装置 | |
JP5422368B2 (ja) | サーボ制御装置 | |
JP2013244540A (ja) | 産業用ロボットの重力たわみ角補正方法および装置 | |
JP2014164498A (ja) | 制御システム、外乱推定システム、制御方法、制御プログラム及び設計方法 | |
Hussein et al. | Control of a two-link (rigid-flexible) manipulator | |
JP7185495B2 (ja) | 減速機システム、駆動ユニットへの指令値の補正方法、補正データの生成方法、及び減速機システムの製造方法 | |
JP5869991B2 (ja) | 駆動装置 | |
JP2000148210A (ja) | ゲイン算出装置 | |
JP4090666B2 (ja) | 数値制御システムおよびそれに用いられる位置指令値補正装置 | |
JP2024017883A (ja) | 動力学トルク補償に基づく制御方法及び制御装置 | |
JP2003071760A (ja) | 組立ロボット | |
Ambike et al. | A methodology for implementing the curvature theory approach to path tracking with planar robots | |
JP2008257571A (ja) | サーボ制御方法 | |
JP2006335529A (ja) | 位置決め制御装置 | |
JP3600025B2 (ja) | 形状測定装置 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20100430 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20101215 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20110104 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20110302 |
|
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: 20110906 |
|
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20110927 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20141007 Year of fee payment: 3 |
|
R151 | Written notification of patent or utility model registration |
Ref document number: 4836872 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20141007 Year of fee payment: 3 |