JP2002178237A - パラレルリンク機構の制御方法 - Google Patents

Abstract

(57)【要約】 【課題】 安定性を損なうことなく応答性の低下を防止
できるパラレルリンク機構の制御方法及び制御装置を提
供する。 【解決手段】 各アーム毎に作用する荷重を求め（Ｓ１
２８）、荷重に応じて各アームを駆動する各サーボユニ
ットへの駆動力を指令値に加算する。このため、安定性
を損なうことなく、トラベリングプレートの移動に伴う
各アームの作用荷重の変動に起因する応答性能の低下を
防止することができる。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、工具ユニットの装
着されるトラベリングプレートを複数のアームで駆動す
るパラレルリンク機構の制御方法及び制御装置に関する
ものである。

【０００２】

【従来の技術】パラレルリンク機構を用いた工作機械と
して例えば、特開平９−１０６６８４号がある。このパ
ラレルリンク機構は、図13に示すように外部に固定され
る基台１１１と、工具Ｔを取り付けるトラベリングプレ
ート１１２と、基台１１１およびトラベリングプレート
１１２を連結する６本のアーム１１４とから構成されて
いる。アーム１１４は、ロッド１１５およびガイド１２
０から構成される。このガイド１２０には、ロッド１１
５を駆動するためのモータ１２５Ｕ、１２５ｕ、１２５
Ｖ、１２５ｖ、１２５Ｗ、１２５ｗが配設されている。
該モータには、それぞれモータ位置検出用エンコーダ１
３１Ｕ、１３１ｕ、１３１Ｖ、１３１ｖ、１３１Ｗ、１
３１ｗが取り付けられている。

【０００３】該パラレルリンク機構においては、各サー
ボモータに取り付けたモータ位置検出用エンコーダから
の出力により、６軸でそれぞれ独立してフィードバック
制御を行っている。この６軸のサーボ系の、後述する制
御パラメータは、図13中に実線で示す原点位置で調整
し、同一の値が用いられる。

【０００４】

【発明が解決しようとする課題】パラレルリンク機構
は、図中に鎖線で示す原点位置から大幅に離れた位置へ
移動させる際に、軌跡精度が劣化するという問題が生じ
ていた。この原因を、本発明者は、トラベリングプレー
トの位置及び姿勢によって各軸に作用する荷重が変化す
るためであると知見した。即ち、図中に実線で示す原点
位置においては、各軸に均等に負荷が加わっているのに
対して、鎖線で示す位置では、各軸に加わる荷重は大幅
に異なる。従来の比例制御では、この荷重の変動に応じ
て直接駆動力を増減する機能がないため、加わる荷重が
増加していく場合には駆動力が不足して応答が遅れ気味
になり、その結果、各軸の移動量の合成で得られる軌跡
精度の劣化を生じる。

【０００５】このような応答の遅れが軌跡精度に影響し
ないよう、各軸の比例ゲインを非常に大きく設定するこ
とも考えられるが、その結果、外乱等に過敏に反応する
ようになり、サーボ系が不安定になってしまうという問
題が生じる。

【０００６】一方、シリアルリンクロボットにおいて
は、負荷の変動に応じて制御方法を変える技術が存在す
るが、シリアルリンクロボットでは各軸に作用する負荷
は他の軸の影響を受けない。このため、他の軸の影響を
受けるパラレルリンク機構に、上記シリアルリンクロボ
ットの技術を転用することは不可能である。

【０００７】請求項１、２の発明は、上述した課題を解
決するためになされたものであり、その目的とするとこ
ろは、サーボ系の安定性を損なうことなく位置決め精度
の低下を防止できるパラレルリンク機構の制御方法を提
供することにある。

【０００８】

【課題を解決するための手段】上記目的を達成するた
め、請求項１は、外部に固定される基台と、工具ユニッ
トの装着されるトラベリングプレートと、前記基台と前
記トラベリングプレートとを連結する複数のアームと、
前記複数のアームを駆動する複数のアクチュエータと、
前記複数の各アクチュエータをそれぞれ駆動する複数の
サーボユニットと、前記サーボユニットへ指令を与え前
記アクチュエータを駆動してし、前記トラベリングプレ
ートの位置及び姿勢を制御するための指令を前記サーボ
ユニットへ与える制御装置と、を備えたパラレルリンク
機構の制御方法であって、前記制御装置は、前記サーボ
ユニットへ与えられる指令によって前記各アーム毎に作
用する荷重を求める荷重演算ステップと、前記各アーム
を駆動する前記サーボユニットの電流指令値に前記求め
た荷重に応じた荷重補正値を前記各アームを駆動する前
記サーボユニットの電流指令値に加算する加算ステップ
と、外部に固定される基台と、工具ユニットの装着され
るトラベリングプレートと、前記基台と前記トラベリン
グプレートとを連結する複数のアームと、前記複数のア
ームを駆動する複数のアクチュエータと、前記各アクチ
ュエータを駆動する複数のサーボユニットと、前記サー
ボユニットへ指令を与え前記アクチュエータを制御する
制御装置と、を備えたパラレルリンク機構の制御方法で
あって、各アーム毎に作用する荷重を指令値に基づき求
めるステップと、前記各アームを駆動する前記サーボユ
ニットへ、適切な変換係数を掛けて前記求めた荷重に応
じた荷重補正値を電流指令値に変換して、電流指令値に
加算するステップと、を備えることを技術的特徴とす
る。

【０００９】また、請求項２では、請求項１において、
前記アーム毎に作用する荷重を求めるステップ荷重演算
ステップを、前記サーボユニットへ与えられる指令と前
記各アーム毎に作用する荷重との関係をから予め設定し
たされたマップを検索し、荷重を求めること指令値から
予め設定されたマップを検索して荷重補正指令を求める
ことを技術的特徴とする。

【００１０】請求項１の発明では、各アーム毎に作用す
る荷重を求め、荷重に応じて各アームを駆動する各サー
ボユニットの電流指令値を加算するため、トラベリング
プレートの移動に伴う各アームの作用荷重の変動に起因
する応答性能の低下を防止できる。また、各サーボユニ
ットの比例ゲインを大きくすることなく用いることがで
きるので、高速性と安定性を両立することが可能にな
る。

【００１１】請求項２の発明では、前記アーム毎に作用
する荷重を求めるステップを、指令値から予め設定され
たマップを検索して求めるようにしたため、演算に要す
る時間を短縮できる。

【００１２】

【発明の実施の形態】以下、本発明のパラレルリンク機
構の制御方法、及び制御装置の実施形態について図を参
照して説明する。図１は第１実施形態のパラレルリンク
機構１０を適用した工作機械全体の構成を示した図であ
る。パラレルリンク機構１０は工作物Ｗを載置するテー
ブル５２を有した門型のフレーム５０の天井に支持柱５
１を介して取り付けられており、このパラレルリンク機
構１０に取り付けられた工具Ｔを所望の位置に移動して
工作物Ｗを加工するものである。

【００１３】次に、図２に基づきパラレルリンク機構１
０について詳細に説明をする。図２に示すようにパラレ
ルリンク機構１０は、主に、支持柱５１によって外部に
固定される基台１１と、ドリルやエンドミル等の工具Ｔ
を駆動する工具ユニットＵを載置するトラベリングプレ
ート１２と、基台１１および上記トラベリングプレート
１２を連結する６本のアーム１４とから構成されてい
る。

【００１４】基台１１は６角形状の部材であり、周囲に
３つの支持部１１ａが等間隔で設けられており、この支
持部１１ａにアーム１４が２本ずつ合計６本連結されて
いる。アーム１４はロッド１５およびガイド２０から構
成されており、６本とも同じ構成となっている。

【００１５】ガイド２０はベース２２、スライドテーブ
ル２６、ボールネジ２４およびモータ２５Ｕ、２５ｕ、
２５Ｖ、２５ｖ、２５Ｗ、２５ｗから構成されている。
該モータ２５Ｕ、２５ｕ、２５Ｖ、２５ｖ、２５Ｗ、２
５ｗには、それぞれモータ位置検出用エンコーダ３１
Ｕ、３１ｕ、３１Ｖ、３１ｖ、３１Ｗ、３１ｗが取り付
けられている。ベース２２は断面形状がコ型をした部材
であり、それぞれのベース２２は基台１１に対して所定
角度α（例えば４５度）傾斜して放射状に基台１１に固
定されている。このベース２２にはその長手方向にスラ
イドテーブル２６が摺動可能に支持されている。また、
ベース２２にはスライドテーブル２６の図略のナットと
螺合するボールネジ２４が回動可能に支持されており、
ベース２２に固定され上記ボールネジ２４に連結される
モータ２５Ｕ、２５ｕ、２５Ｖ、２５ｖ、２５Ｗ、２５
ｗ（駆動装置）を駆動することにより、ボールネジ２４
を回動し、結果としてスライドテーブル２６をベース２
２の長手方向に移動するようになっている。

【００１６】上述したスライドテーブル２６にはそれぞ
れロッド１５がボールジョイント１６（対偶）により連
結され、ボールジョイント１６を支点としてロッド１５
はスライドテーブル２６に対して３次元方向に揺動可能
となっている。また、各ロッド１５の他端はトラベリン
グプレート１２にボールジョイント１７（対偶）にて連
結され、ボールジョイント１７を支点としてロッド１５
はトラベリングプレート１２に対して３次元方向に揺動
可能となっている。

【００１７】トラベリングプレート１２は上記ロッド１
５の他端がボールジョイント１７により同一平面上に連
結されており、下部にはドリルやエンドミル等の工具Ｔ
を駆動する工具ユニットＵが取り付けられている。以上
の構成により、後述するの制御装置より動作指令を与え
ることによって、ガイド２０のモータ２５Ｕ、２５ｕ、
２５Ｖ、２５ｖ、２５Ｗ、２５ｗを個別に駆動して６本
のロッド１５をそれぞれ独立して揺動させる。すると、
この６本のロッド１５の揺動の組み合わせにより、トラ
ベリングプレート１２を６自由度制御（位置および姿勢
制御）することができる。つまり、各支持部１１ａに支
持された２本１組のアーム１４を同期して３組のアーム
１４を個別に駆動することによりトラベリングプレート
の位置を決定し、各支持部１１ａに固定された２本１組
のアーム１４のうちの各１本、即ち合計３本のアーム１
４を駆動することによりトラベリングプレート１２の姿
勢を決定することができるようになっている。そして、
トラベリングプレート１２に取り付けられた工具を所望
の位置および姿勢に移動し、工作物の加工を行うように
なっている。ここで、該トラベリングプレート１２及び
工具ユニットＵの重量は、約２００Ｋｇある。

【００１８】引き続き、図３を参照して制御装置７０の
構成について説明する。制御装置７０は、ＣＰＵ７１、
メモリ７２、インタフェイス７３、７４から構成されて
いる。メモリ７２には実加工処理を実行するためのプロ
グラムが記憶されている。インタフェイス７４には、後
述する加工データ等を入力するキーボード７６、加工デ
ータや現在のパラレルリンク機構１０の状態等を表示す
る画像表示装置（ＣＲＴ）７７、加工データを記憶する
外部記憶装置（例えばハードディスク）７３が接続され
ている。

【００１９】インタフェイス７３には、上述したサーボ
モータ２５Ｕ、２５ｕ、２５Ｖ、２５ｖ、２５Ｗ、２５
ｗを駆動するデジタルサーボユニット８１〜８６が接続
されている。各デジタルサーボユニット８１〜８６は、
ＣＰＵ７１からの荷重補正指令に基づいて、駆動力を調
整すると共に、ＣＰＵ７１からの指令値（目標値）に基
づいてサーボモータ２５Ｕ、２５ｕ、２５Ｖ、２５ｖ、
２５Ｗ、２５ｗをそれぞれ駆動し、各モータ位置検出用
エンコーダ３１Ｕ、３１ｕ、３１Ｖ、３１ｖ、３１Ｗ、
３１ｗからの出力によってフィードバック制御を行う。
そして、サーボモータ２５Ｕ、２５ｕ、２５Ｖ、２５
ｖ、２５Ｗ、２５ｗによって駆動されるそれぞれのスラ
イドテーブル２６を所望の位置にそれぞれ移動すること
により、結果として、６本のロッド１５を介して連結さ
れるトラベリングプレート１２を所望の位置及び姿勢に
制御するようになっている。

【００２０】このデジタルサーボユニット８１のサーボ
系の構成について、図４を参照して説明する。数値制御
装置７０からの位置指令値とエンコーダ３１Ｕからの現
在位置とが比較されて位置偏差が算出され、この位置偏
差は一方で微分器９３により微分され微分ゲインＫｄｅ
が掛けられると共に、他方で積分器９４により積分され
積分ゲインＫｉｎが掛けられる。また、位置指令は一方
でフィードフォワードループの微分器９１により微分さ
れ速度フィードフォワードゲインＫｖｆが掛けられると
共に、他方で微分器９２により２回微分され加速度フィ
ードフォワードゲインＫａｆが掛けられる。そして、こ
れらが全て加算され、乗算器９５で比例ゲインＫｐｒが
掛けられて速度指令が得られる。この速度指令は、エン
コーダ３１Ｕからの現在位置が微分器９６で微分されダ
ンピング係数Ｋｄｕが掛けられて得られる現在速度と比
較され、速度偏差が算出される。この速度偏差に乗算器
９７によって比例ゲインＫｖｒを掛けられたものに、数
値制御装置７０からの後述する荷重補正指令に、乗算器
９８で駆動力をサーボユニットの電流指令値に変換する
係数Ｋｔｉを掛けられたものが加算されて、サーボユニ
ットの最終段である電流ループへ電流指令として入力さ
れてサーボモータが駆動される。ここで係数Ｋｔｉは、
ボールねじ２４が駆動方向に発生する力とサーボモータ
２５の制御電流を関係づけるもので、例えば、前記ボー
ルネジのリードと、回転運動から直線運動への力の変換
効率を、それぞれＬとηとし、サーボモータのトルク定
数をＫｔとした時には、Ｋｔｉ＝ Ｌ／（２πηＫｔ）
で表される係数である。

【００２１】引き続き、前記各サーボユニットへ指令さ
れる荷重補正指令、すなわち各ユニットの作用荷重の算
出方法について説明する。ここでは代表の座標として、
原点位置での作用荷重を求める方法について説明する。
まず、制御装置７０は、原点（直交座標系の指令点）Ｐ
0 を入力する。ここで、原点とは、図２で示すパラレル
リンク機構の中央位置を意味する。次に、原点Ｐ0 を逆
変換して出力座標系（アクチュエータ座標）の原点Ｑ0
を求める。

【００２２】当該逆変換について、当該処理のサブルー
チンを示す図8を参照して説明する。先ず、直交座標系
の指令点（原点）Ｐ0 （Ｘ0 ，Ｙ0 ，Ｚ0 ，Ａ0 ，Ｂ0
，Ｃ0 ）を入力する（Ｓ１５０）。次に、この直交座
標系で指定された指令点（Ｘ0，Ｙ0 ，Ｚ0 ，Ａ0 ，Ｂ0
，Ｃ0 ）を各デジタルサーボユニット８１，８２，８
３，８４，８５，８６に出力される出力座標系の指令値
（Ｕ0 ，ｕ0 ，Ｖ0 ，ｖ0 ，Ｗ0 ，ｗ0 ）に座標変換す
る（Ｓ１５２）。

【００２３】この逆変換について図5の説明図を参照し
て更に詳細に説明する。制御装置７０への指令は指令点
ＰN （ＸN ，ＹN ，ＺN ，ＡN ，ＢN ，ＣN ）の形で入
力される。ここで、Ｘ，Ｙ，Ｚは直交座標系における位
置を表し、Ａ，Ｂ，ＣはＸ，Ｙ，Ｚ軸周りの回転を表し
ている。この直交座標系の指令値では、６軸のアーム１
５Ｕ〜１５ｗを駆動するサーボモータ２５Ｕ〜２５ｗを
制御できないため、各サーボモータを制御するための指
令値（出力座標系）へ変換する。これを逆変換とよぶ。

【００２４】出力座標系では、図5中に示すように各ボ
ールネジ２４Ｕ，２４ｕ，２４Ｖ，２４ｖ，２４Ｗ，２
４ｗの端点（基台１１との交点）をそれぞれの原点Ｏ
Ｕ，Ｏｕ，ＯＶ，Ｏｖ，ＯＷ，Ｏｗとし、各ボールネジ
２４Ｕ，２４ｕ，２４Ｖ，２４ｖ，２４Ｗ，２４ｗの方
向をＵ軸、ｕ軸、Ｖ軸、ｖ軸、Ｗ軸、ｗ軸とした６軸か
ら構成され、（Ｕ，ｕ，Ｖ，ｖ，Ｗ，ｗ）と表記する。

【００２５】この直交座標系の指令値（Ｘ0 ，Ｙ0 ，Ｚ
0 ，Ａ0 ，Ｂ0 ，Ｃ0 ）から出力座標系の指令値（Ｕ0
，ｕ0 ，Ｖ0 ，ｖ0 ，Ｗ0 ，ｗ0 ）への変換について
Ｕ軸（アーム１５Ｕ）を例に挙げて説明する。ここで、
他の出力座標系（ｕ，Ｖ，ｖ，Ｗ，ｗ）への変換処理
は、それぞれ同様の演算により行われる。

【００２６】まず、所定の角度Ｋにて基台１１に固定さ
れているＵ軸（ボールネジ２４Ｕ）の直交座標系におけ
る直線の方程式を求める。次に、第１番目の指令点、こ
こでは、原点Ｐ0 （Ｘ0 ，Ｙ0 ，Ｚ0 ，Ａ0 ，Ｂ0 ，Ｃ
0 ）にトラベリングプレート１２が移動された場合のボ
ールジョイント１７Ｕの座標ＴＵを直交座標系にて算出
し、さらに、この座標ＴＵを中心とした半径Ｒ（ロッド
１５Ｕの長さＲ）の球の方程式を求める。そして、上記
求めた直線の方程式と球の方程式から交点を算出し、こ
の交点とＵ軸の原点ＯＵとの距離を求めて、この値を出
力座標系に変換された指令値Ｕ0 とする。

【００２７】以上の演算をｕ軸、Ｖ軸、ｖ軸、Ｗ軸、ｗ
軸についてそれぞれ同様に行い、出力座標系における指
令点、ここでは原点Ｑ0 （Ｕ0 ，ｕ0 ，Ｖ0 ，ｖ0 ，Ｗ
0 ，ｗ0 ）を得る。なお、上述した出力座標系への変換
は各ボールジョイント１６Ｕが各ボールネジ２４Ｕ上を
移動する図5のモデルについて説明されているため、実
際に行われる座標変換においては、ボールネジ２４Ｕか
らボールジョイント１６Ｕまでの距離を考慮して座標変
換を行う必要がある。

【００２８】前記各サーボユニットへ指令される荷重補
正指令の算出方法について説明を続ける。上述した逆変
換処理に続き、原点Ｐ0 における各サーボユニットに分
配される作用荷重を演算する。この処理について、当該
荷重演算処理のサブルーチンを示す図9を参照して説明
する。

【００２９】まず、直交座標（ＸＹＺ座標）系の指令値
Ｐ0 と出力座標（アクチュエータ座標）系の指令値Ｑ0
とを入力する（Ｓ１７０）。そして、各ロッド１５Ｗ〜
１５ｕの位置・姿勢を演算する（Ｓ１７２）。次に、後
述するつりあいの方程式から、各ロッドにかかる力を演
算する（Ｓ１７４）。

【００３０】この６軸の作用荷重の算出について図6を
参照して説明する。図6は、トラベリングプレート１２
と各ロッドに加わる作用荷重とを示している。先ず、工
具先端の位置・姿勢が指令されると、上記逆変換によっ
て各ロッドの位置・傾きが計算できる。次に、各ロッド
に加わる力を求める。ここで、力の方向は、ロッドの位
置・傾きにより機構的に決定されるため、力の大きさの
みを以下の計算によって求める。

【００３１】先ず、６軸の各ロッドの方向ベクトルをそ
れぞれe1、e2、e3、e4、e5、e6とし、トラベリングプレ
ート１２の中心ＯＴからロッドの結合点（ボールジョイ
ント）Ｊ１〜Ｊ６へのベクトルをR1〜R6とし、力の大き
さをf1〜f6として、トラベリングプレート１２及び工具
ユニットＵに作用する重力ベクトルをＦとし、トラベリ
ングプレート１２の中心ＯＴから工具先端へのベクトル
をＲ0 として、つり合いの方程式（数１、数２）をたて
る。

【００３２】力の並進のつり合いから次の数１が表せ
る。

【数１】 f1e1 + f2e2 + f3e3 + f4e4 + f5e5 + d6e6 = F

【００３３】他方、ＯＴ回りのモーメントのつり合いか
ら次の数２が表せる。

【数２】f1e1×R1 + f2e2×R2 + f3e3×R3 + f4e4×R4
+ f5e5×R5 + f6e6×R6 =F×Ｒ０ ここで、Ａ×Ｂは、ＡとＢとの外積の計算を示してい
る。

【００３４】上記数１及び数２をＸ、Ｙ、Ｚ方向にそれ
ぞれ３式に分解することで、６個の未知数を含む６個の
方程式が立つ。この方程式から各ロッドに加わる力を算
出する。引き続き、算出した力をアクチュエータ方向に
換算（角度に応じた係数を掛ける）、即ち、算出した力
を各軸に係る荷重に変換することで、作用荷重を算出す
る（図9に示すＳ１７６）。

【００３５】次に、第１実施形態のパラレルリンク機構
の制御装置７０による実際の金型加工処理について、図
7を参照して説明する。先ず、パラレルリンク機構１０
による金型加工用のプログラムを入力する（Ｓ１２
０）。次に、指令点の番号を“１”に初期化し（Ｓ１２
２）、第Ｎ番目の指令点（ここでは、第１番目）を入力
する（Ｓ１２４）。引き続き、直交座標系の指令点ＰN
（ＸN 、ＹN 、ＺN 、ＡN、ＢN 、ＣN ）をアクチュエ
ータ座標系ＱN （ＵN 、ｕN 、ＶN 、ｖN 、ＷN 、ｗN
）へ、図8を参照して上述したように逆変換する（Ｓ１
２６）。そして、指令点ＰN における各軸の作用荷重Ｗ
Nを、図9を参照して上述したように演算する（Ｓ１２
８）。その後、各デジタルサーボユニット８１〜８６
へ、アクチュエータ座標の指令点ＱN を位置指令とし
て、各軸の作用荷重ＷNを荷重補正指令値として出力
し、サーボモータを駆動してパラレルリンク機構１０を
指令点ＰN へ移動させる（Ｓ１３０）。その後、指令値
（指令点）ＰN が最後かを判断し（Ｓ１３２）、指令値
ＰN が最後でない限り（Ｓ１３２がＮｏ）、ステップ１
３４へ移行し、指令値を１インクリメントしてステップ
１２４に戻り処理を続ける。他方、指令値ＰN が最後の
場合には（Ｓ１３２がＹｅｓ）、処理を終了する。

【００３６】この第１実施形態のパラレルリンク機構の
制御方法及び制御装置においては、各アーム毎に作用す
る荷重を求め、荷重に応じた各アームの駆動力を各サー
ボユニットの電流指令値に直接加算するため、トラベリ
ングプレートの移動に伴う各アームの作用荷重の変動に
起因する応答性能の低下を防止できる。即ち、サーボユ
ニットにとって外乱として働く、前記各アーム毎に作用
する荷重を相殺する力を電流指令に直接加算して制御す
ることで、見かけ上、外乱となる前記荷重がない均一の
条件下でサーボユニットは動作でき、これより各サーボ
ユニットの制御パラメータを変更することなく、高速性
と安定性を両立することが可能になる。

【００３７】引き続き、本発明の第２実施形態に係るパ
ラレルリンク機構の制御方法及び制御装置について、図
10〜図12を参照して説明する。この第２実施形態のパラ
レルリンク機構の機械的構成は、図２を参照して上述し
た第１実施形態と同様である。但し、第１実施形態にお
いては、制御装置７０が指令値（指令点）から各アーム
毎に作用荷重を演算したのに対して、第２実施形態で
は、予めパラレルリンク機構の各位置及び姿勢に於ける
荷重補正指令値をテーブル（マップ）として保持してお
き、該テーブルを検索することで荷重補正指令値を求め
られるようにする。即ち、図12に示すようにパラレルリ
ンク機構１０にて送り得る工具Ｔの先端位置を６×６×
６のブロックに分割し、各ブロック毎に最適な荷重補正
値を求めてテーブルを作成しておく。

【００３８】この第２実施形態のパラレルリンク機構の
制御方法及び制御装置におけるテーブルの作成処理につ
いて、図11を参照して説明する。制御装置７０は、先
ず、マップ測定用の走査プログラムを入力する（Ｓ２３
０）。このマップ測定用の走査プログラムは、図12に示
した６×６×６のブロックのそれぞれへの移動指令と、
各ブロックでの適当な数の姿勢変更指令からなる。次
に、指令点の番号を“１”に初期化し（Ｓ２３２）、第
Ｎ番目の指令点｛ここでは、第１番目、例えば、図12中
のブロック（ｌ１、ｍ１、ｎ１）中の指令点｝を入力す
る（Ｓ２３４）。次に、直交座標系の指令点ＰN （ＸN
、ＹN 、ＺN 、ＡN 、ＢN 、ＣN ）をアクチュエータ
座標系ＱN （ＵN 、ｕN 、ＶN 、ｖN、ＷN 、ｗN ）に
図8を参照して上述したように逆変換する（Ｓ２３
６）。そして、指令点ＰN における各軸の作用荷重ＷN
を図9を参照して上述したようにも演算し（Ｓ２３
８）、当該指定点ＰNの位置（ブロック（ｌ１、ｍ１、
ｎ１））及び当該姿勢に対応する作用荷重ＷNを荷重補
正指令のマップの値として保持する（Ｓ２３８）。その
後、指令値（指令点）ＰN が最後かを判断し（Ｓ２４
０）、指令値ＰN が最後でない限り（Ｓ２４０がＮ
ｏ）、ステップ２４２へ移行し、指令値を１インクリメ
ントしてステップ２３４に戻り処理を続ける。他方、指
令値ＰN が最後の場合には（Ｓ２４０がＹｅｓ）、処理
を終了する。

【００３９】次に、上述した処理により作成したマップ
を用いる第２実施形態のパラレルリンク機構の制御装置
７０による実際の金型加工処理について、図10を参照し
て説明する。制御装置７０は、先ず、パラレルリンク機
構１０による金型加工用のプログラムを入力する（Ｓ２
１０）。次に、指令点の番号を“１”に初期化し（Ｓ２
１２）、第Ｎ番目の指令点（ここでは、第１番目）を入
力する（Ｓ２１４）。そして、直交座標系の指令点ＰN
に対応する荷重補正指令値を、マップを検索することに
より読み込む（Ｓ２１６）。その後、直交座標系の指令
点ＰN （ＸN 、ＹN 、ＺN 、ＡN 、ＢN 、ＣN ）をアク
チュエータ座標系ＱN （ＵN 、ｕN 、ＶN 、ｖN 、ＷN
、ｗN ）へ逆変換し（Ｓ２１８）、アクチュエータ座
標の指令点ＱN と荷重補正値を各デジタルサーボユニッ
ト８１〜８６へ出力し、サーボモータを駆動してパラレ
ルリンク機構１０を指令点ＰN へ移動させる。その後、
指令値（指令点）ＰN が最後かを判断し（Ｓ２２２）、
指令値ＰN が最後でない限り（Ｓ２２２がＮｏ）、ステ
ップ２２４へ移行し、指令値を１インクリメントしてス
テップ２１４に戻り処理を続ける。他方、指令値ＰN が
最後の場合には（Ｓ２２２がＹｅｓ）、処理を終了す
る。ここで、テーブルの作成時においては、走査プログ
ラムの位置指令は図12に示した各ブロックの中心位置と
し、実際の加工処理においては、移動指令が当該ブロッ
ク内であるときは全て、ブロックの中心で求めた制御パ
ラメータを用いるようにすることもできるし、走査プロ
グラムの位置指令を図12に示した各ブロックの頂点、す
なわち格子点とし、実際の加工処理においては、移動指
令が２つ（あるいはそれ以上）格子点の間の位置である
ときは、その２つ（あるいはそれ以上）の格子点で求め
た各制御パラメータの値を補間して求めるようにするこ
ともできる。

【００４０】この第２実施形態のパラレルリンク機構の
制御方法及び制御装置においては、第１実施形態の効果
に加えて、予め設定されたマップを検索して各アーム毎
の制御パラメータを求めるため、演算に要する時間を短
縮することができる。

【００４１】

【発明の効果】請求項１の発明では、各アーム毎に作用
する荷重を求め、荷重に応じて各アームを駆動する各サ
ーボユニットの電流指令値を加算するため、トラベリン
グプレートの移動に伴う各アームの作用荷重の変動に起
因する応答性能の低下を防止できる。また、各サーボユ
ニットの制御パラメータを変更することなく用いること
ができるので、高速性と安定性を両立することが可能に
なる。

【００４２】請求項２の発明では、予め設定されたマッ
プを検索して各アーム毎に作用する荷重を求めるため、
演算に要する時間を短縮できる。

【図面の簡単な説明】

【図１】本発明の第１実施形態に係る制御装置により制
御されるパラレルリンク機構の機械的構成を示す斜視図
である。

【図２】図１に示すパラレルリンク機構の機械的構成を
示す斜視図である。

【図３】本発明の第１実施形態に係る制御装置の構成を
示すブロック図である。

【図４】図３に示すデジタルサーボユニットの制御系を
示すブロック図である。

【図5】パラレルリンク機構の逆変換処理を示す説明図
である。

【図6】トラベリングプレートと各ロッドに加わる作用
荷重を示す説明図である。

【図7】第１実施形態の制御装置の主処理を示すフロー
チャートである。

【図8】逆変換処理のサブルーチンを示すフローチャー
トである。

【図9】荷重演算処理のサブルーチンを示すフローチャ
ートである。

【図10】第２実施形態の制御装置の主処理を示すフロー
チャートである。

【図11】第２実施形態の制御装置のマップ作成処理を示
すフローチャートである。

【図12】第２実施形態のパラレルリンク機構の説明図で
ある。

【図13】従来技術に係るパラレルリンク機構を示す説明
図である。

【符号の説明】

１０ パラレルリンク機構 １４ アーム １５ ロッド ２０ ガイド ２５Ｕ〜２５ｗ サーボモータ ３１Ｕ〜３１ｗ モータ位置検出用エンコーダ ７０ 制御装置 ７２ メモリ ７３ 外部記憶装置 ７４ ＣＰＵ ７６ 入力装置 ７７ ＣＲＴ ８１Ｕ〜８１ｕ デジタルサーボユニット（サーボユニ
ット） Ｔ 工具 Ｕ 工具ユニット

───────────────────────────────────────────────────── フロントページの続き Ｆターム(参考） 3C001 KA01 KA06 KB02 KB04 TA02 TB06 TC05 TD03 3C048 AA01 BB20 BC01 DD00 EE00 EE08

Claims (2)

【特許請求の範囲】
1. 【請求項１】 外部に固定される基台と、工具ユニット
   の装着されるトラベリングプレートと、前記基台と前記
   トラベリングプレートとを連結する複数のアームと、前
   記複数のアームを駆動する複数のアクチュエータと、前
   記各複数のアクチュエータをそれぞれ駆動する複数のサ
   ーボユニットと、前記サーボユニットへ指令を与え前記
   アクチュエータを駆動してし、前記トラベリングプレー
   トの位置及び姿勢を制御するための指令を前記サーボユ
   ニットへ与える制御装置と、を備えたパラレルリンク機
   構の制御方法であって、前記制御装置は、前記サーボユ
   ニットへ与えられる指令によって前記各アーム毎に作用
   する荷重を指令値に基づき求める荷重演算ステップと、
   前記求めた荷重に応じた荷重補正値を前記各アームを駆
   動する前記サーボユニットの電流指令値に前記求めた荷
   重に応じた荷重補正値、求めた荷重に応じて算出した荷
   重補正値を適切な係数を掛けて加算する加算して制御す
   るステップと、を備えることを特徴とするパラレルリン
   ク機構の制御方法。
2. 【請求項２】 前記アーム毎に作用する荷重を求める荷
   重演算ステップでは、前記サーボユニットへ与えられる
   指令値と前記各アーム毎に作用する荷重との関係をから
   予め設定したされたマップを検索し、荷重補正指令を求
   めることを特徴とする請求項１に記載のパラレルリンク
   機構の制御方法。