JP2002091568A

JP2002091568A - パラレルリンク機構の制御方法及び制御装置

Info

Publication number: JP2002091568A
Application number: JP2000282145A
Authority: JP
Inventors: Hiromitsu Ota; 浩充太田
Original assignee: Toyoda Koki KK
Current assignee: Toyoda Koki KK
Priority date: 2000-09-18
Filing date: 2000-09-18
Publication date: 2002-03-29

Abstract

(57)【要約】【課題】位置決め精度を向上し得るパラレルリンク機
構の制御方法及び制御装置を提供する。【解決手段】本実施形態に係るパラレルリンク機構の
制御方法では、ステップＳ１０、Ｓ１２により、所定の
機構パラメータＰ₀に基づいて工具ユニットの目標位置
に対応する複数のアクチュエータのアクチュエータ座標
ｄ₀を求め、ステップＳ１４、Ｓ１６、Ｓ１８により、
このアクチュエータ座標ｄ₀に基づいて目標位置及び姿
勢に応じてトラベリングプレート等に加わる力を求め、
ステップＳ２２により、求めた力に基づいてトラベリン
グプレート等に生ずる弾性変形による変形量を求め、ス
テップＳ２４により、弾性変形量に基づいて機構パラメ
ータＰを修正し、ステップＳ２６により、修正した機構
パラメータＰに基づいて工具ユニットの目標位置に対応
する複数のアクチュエータのアクチュエータ座標ｄ₁を
求める。

Description

【発明の詳細な説明】

【０００１】

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

【０００２】

【従来の技術】パラレルリンク機構を用いた工作機械と
して、例えば本出願人に係る特開平９−１０６６８４号
公報に開示されるものがある。このパラレルリンク機構
は、図１８に示すように外部に固定される基台１１１
と、工具Ｔを取り付けるトラベリングプレート１１２
と、基台１１１及びトラベリングプレート１１２を連結
する６本のアーム１１４とから構成されている。

【０００３】アーム１１４は、ロッド１１５及びガイド
１２０から構成され、両者の接続にはジョイント１１６
が用いられている。またロッド１１５とトラベリングプ
レート１１２との接続にも同様にジョイント１１７が用
いられている。このガイド１２０には、ボールねじ１２
４を介してロッド１１５を駆動するためのモータ１２５
Ｕ、１２５ｕ、１２５Ｖ、１２５ｖ、１２５Ｗ、１２５
ｗ（図中にはモータ１２５ｕ、１２５Ｖ、１２５ｖ、１
２５ｗのみ示す）が配設されている。またこのモータに
は、それぞれモータ位置検出用エンコーダ１３１Ｕ、１
３１ｕ、１３１Ｖ、１３１ｖ、１３１Ｗ、１３１ｗ（図
１８では、エンコーダ１３１ｕ、１３１Ｖ、１３１ｖ、
１３１ｗのみ示す）が取り付けられている。

【０００４】そして、このようなパラレルリンク機構に
おいては、各サーボモータに取り付けたモータ位置検出
用エンコーダからの出力により、６軸でそれぞれ独立し
てフィードバック制御を行っている。この６軸のサーボ
系の制御パラメータ（比例ゲイン、ダンピング係数等）
は、図１８中にＸで示す原点位置で調整し、同一の値が
用いられる。

【０００５】ところが、かかるパラレルリンク機構等で
は、同図中に実線で示す原点位置から大幅に離れた二点
鎖線で示す位置へ移動させる際に、トラベリングプレー
ト１１２の位置及び姿勢によって各軸に作用する荷重が
変化する。そのため、振動が発生したり、剛性が低下す
るという問題が生じていた。

【０００６】そこで、本願出願人は、さらに同者に係る
特開平１１−２７７４６９号公報に開示される「パラレ
ルリンク機構の制御方法及び制御装置」により、このよ
うな問題を解決した。即ち、各アーム１１４毎に作用す
る荷重を指令値に基づき求め、この荷重に応じて各アー
ム１１４を駆動する各サーボモータの制御パラメータを
変更することにより、トラベリングプレート１１２の移
動に伴う各アームの作用荷重の変動に起因する制御性能
の低下を防止するものとした。また、各アーム１１４毎
に作用する荷重を指令値に基づき求め、この荷重に応じ
たアーム１１４の変形量を求め、このアーム１１４の変
形量に応じて指令値を補正することにより、荷重の影響
による位置決め精度の低下を防止するものとした。

【０００７】

【発明が解決しようとする課題】しかしながら、同公開
公報に開示される「パラレルリンク機構の制御方法及び
制御装置」によると、各アーム１１４毎に作用する荷重
を求め、この求めた荷重に応じて、各サーボモータの制
御パラメータを変更したり、指令値を補正したりする。
そのため、トラベリングプレート１１２の移動に伴う
「各アームの作用荷重の変動」に起因する制御性能の低
下を防止できても、それ以外のものに関して生じる作動
荷重の変動については、それらに起因する制御性能の低
下を防止することはできない。具体的には、例えば図１
８に示すトラベリングプレート１１２、ボールねじ１２
４に関して生じる作動荷重の変動によるものである。

【０００８】また、ジョイント１１６、１１７について
は、図１８に示すようなボールジョイントを用いると、
その構造上、ジョイントの回転角度の範囲が狭くなるた
め、一般には１軸のベアリングジョイントを複数組み合
わせて実現される。そのためトラベリングプレート１１
２や工具Ｔ等の自重によって、トラベリングプレート１
１２、ボールねじ１２４等が弾性変形することに加え
て、ジョイント１１６、１１７を構成する複数のベアリ
ングジョイントも弾性変形を生じるため、このような各
部の弾性変形に伴うものも制御性能を低下させる要因と
なる。つまり、これらの弾性変形が制御性能を低下さ
せ、ひいては位置決め精度の向上を妨げていることを、
本願発明者は知見するに至っている。

【０００９】さらに、トラベリングプレート１１２等の
位置、姿勢によって各軸に分配される負荷バランスが変
化するため、このような変化によってもトラベリングプ
レート１１２、ボールねじ１２４等の弾性変形量が変動
する。例えば、本願発明者が行った実験によると、図１
９に示すように、原点位置でＡ軸（ｘ軸回りの回転角）
の角度を変えたときのロッド１１５の軸方向の負荷は、
その角度が０゜においては小さく均等であったものが、
当該角度の増加につれて大きく不均一になることが確認
されている。これは、トラベリングプレート１１２等の
位置、姿勢による負荷の変化が、各部の弾性変形量の変
化を引き起こし、さらには位置決め精度の低下をも招き
得ることを意味している。

【００１０】本発明は、上述した課題を解決するために
なされたものであり、その目的とするところは、位置決
め精度を向上し得るパラレルリンク機構の制御方法及び
制御装置を提供することにある。

【００１１】

【課題を解決するための手段】上記目的を達成するた
め、請求項１のパラレルリンク機構の制御方法では、外
部に固定される基台、工具ユニットの装着されるトラベ
リングプレート、前記基台と前記トラベリングプレート
を連結する複数のロッド、前記複数のロッドを駆動する
複数のアクチュエータ、前記各アクチュエータを制御す
る複数のサーボユニット及び前記サーボユニットへ指令
を与え前記アクチュエータを制御する制御装置、を備え
たパラレルリンク機構の制御方法であって、所定の機構
パラメータに基づいて前記工具ユニットの目標位置に対
応する前記複数のアクチュエータの位置情報を求める第
１ステップと、前記第１ステップにより求めた複数のア
クチュエータの位置情報に基づいて、目標位置及び姿勢
に応じて前記トラベリングプレート、前記複数のロッド
及び前記複数のアクチュエータにそれぞれ加わる力を求
める第２ステップと、前記第２ステップによる求めた力
に基づいて、前記トラベリングプレート、前記複数のロ
ッド及び前記複数のアクチュエータのそれぞれに生ずる
弾性変形による変形量を求める第３ステップと、前記第
３ステップにより求めた弾性変形量に基づいて、前記機
構パラメータを修正する第４ステップと、前記第４ステ
ップにより修正した機構パラメータに基づいて、前記工
具ユニットの目標位置に対応する前記複数のアクチュエ
ータの位置情報を求める第５ステップと、を含むことを
技術的特徴とする。

【００１２】また、請求項２のパラレルリンク機構の制
御方法では、請求項１において、前記基台と前記ロッド
との間には、回転軸の方向が異なる複数の基台側ジョイ
ントが介在し、また前記トラベリングプレートと前記ロ
ッドとの間には、回転軸の方向が異なる複数のＴＰ側ジ
ョイントが介在する請求項１記載のパラレルリンク機構
の制御方法であって、前記第３ステップは、目標位置及
び姿勢に応じて、前記基台側ジョイントおよび前記ＴＰ
側ジョイントにそれぞれ加わる力に基づいて、前記基台
側ジョイント及びＴＰ側ジョイントに生ずる弾性変形に
よる変形量も求めることを技術的特徴とする。

【００１３】さらに、請求項３のパラレルリンク機構の
制御方法では、請求項１または２において、前記第２ス
テップにより求めた力と同ステップにより前回求めた力
との差が、所定値よりも小さくなるまで、前記第２ステ
ップ、第３ステップ、第４ステップ及び第５ステップに
よる各処理を繰り返し行うことを技術的特徴とする。

【００１４】上記目的を達成するため、請求項４のパラ
レルリンク機構の制御装置では、外部に固定される基
台、工具ユニットの装着されるトラベリングプレート、
前記基台と前記トラベリングプレートを連結する複数の
ロッド、前記複数のロッドを駆動する複数のアクチュエ
ータ及び前記各アクチュエータを制御する複数のサーボ
ユニット、を有するパラレルリンク機構の前記サーボユ
ニットに指令を与え前記アクチュエータを制御するパラ
レルリンク機構の制御装置であって、所定の機構パラメ
ータに基づいて前記工具ユニットの目標位置に対応する
前記複数のアクチュエータの位置情報を求める第１位置
情報取得手段と、前記位置情報取得手段により求めた複
数のアクチュエータの位置情報に基づいて、目標位置及
び姿勢に応じて前記トラベリングプレート、前記複数の
ロッド及び前記複数のアクチュエータにそれぞれ加わる
力を求める負荷取得手段と、前記負荷取得手段による求
めた力に基づいて、前記トラベリングプレート、前記複
数のロッド及び前記複数のアクチュエータのそれぞれに
生ずる弾性変形による変形量を求める弾性変形量取得手
段と、前記弾性変形量取得手段により求めた弾性変形量
に基づいて、前記機構パラメータを修正する機構パラメ
ータ修正手段と、前記機構パラメータ修正手段により修
正した機構パラメータに基づいて、前記工具ユニットの
目標位置に対応する前記複数のアクチュエータの位置情
報を求める第２位置情報検出手段と、を備えることを技
術的特徴とする。

【００１５】請求項１の発明では、第１ステップによ
り、所定の機構パラメータに基づいて工具ユニットの目
標位置に対応する複数のアクチュエータの位置情報を求
め、第２ステップにより、第１ステップにより求めた複
数のアクチュエータの位置情報に基づいて、目標位置及
び姿勢に応じてトラベリングプレート、複数のロッド及
び複数のアクチュエータにそれぞれ加わる力を求め、第
３ステップにより、第２ステップにより求めた力に基づ
いて、トラベリングプレート、複数のロッド及び複数の
アクチュエータのそれぞれに生ずる弾性変形による変形
量を求め、第４ステップにより、第３ステップにより求
めた弾性変形量に基づいて、機構パラメータを修正し、
第５ステップにより、第４ステップにより修正した機構
パラメータに基づいて、工具ユニットの目標位置に対応
する複数のアクチュエータの位置情報を求める。即ち、
目標位置及び姿勢に応じてトラベリングプレート、複数
のロッド及び複数のアクチュエータにそれぞれ加わる力
からそれぞれに生ずる弾性変形量に基づいて機構パラメ
ータが修正され、この修正した機構パラメータに基づい
て工具ユニットの目標位置に対応する複数のアクチュエ
ータの位置情報を求める。これにより、位置、姿勢の変
化によって各軸に分配される負荷バランスが変化して
も、このような負荷バランスの変化により生じるトラベ
リングプレート、複数のロッド及び複数のアクチュエー
タの弾性変形を考慮して工具ユニットの目標位置に対応
する複数のアクチュエータの位置情報を求めることがで
きる。

【００１６】請求項２の発明では、第３ステップは、目
標位置及び姿勢に応じて、基台側ジョイントおよびＴＰ
側ジョイントにそれぞれ加わる力に基づいて、基台側ジ
ョイント及びＴＰ側ジョイントに生ずる弾性変形による
変形量も求める。これにより、基台と前記ロッドとの間
には、回転軸の方向が異なる複数の基台側ジョイントが
介在し、またトラベリングプレートとロッドとの間に
は、回転軸の方向が異なる複数のＴＰ側ジョイントが介
在した場合において、位置、姿勢の変化によって各軸に
分配される負荷バランスが変化しても、このような負荷
バランスの変化により生じる基台側ジョイントおよびＴ
Ｐ側ジョイントの弾性変形を考慮して工具ユニットの目
標位置に対応する複数のアクチュエータの位置情報を求
めることができる。

【００１７】請求項３の発明では、第２ステップにより
求めた力と同ステップにより前回求めた力との差が、所
定値よりも小さくなるまで、第２ステップ、第３ステッ
プ、第４ステップ及び第５ステップによる各処理を繰り
返し行う。即ち、所定値よりも小さくなるまで、第２ス
テップ、第３ステップ、第４ステップ及び第５ステップ
による収束計算を行うことができる。これにより、弾性
変形量をより正確に求めることができるので、複数のア
クチュエータの位置情報の精度を高めることができる。

【００１８】請求項４の発明では、第１位置情報取得手
段により、所定の機構パラメータに基づいて工具ユニッ
トの目標位置に対応する複数のアクチュエータの位置情
報を求め、負荷取得手段により、位置情報取得手段によ
り求めた複数のアクチュエータの位置情報に基づいて、
目標位置及び姿勢に応じてトラベリングプレート、複数
のロッド及び複数のアクチュエータにそれぞれ加わる力
を求め、弾性変形量取得手段により、負荷取得手段によ
り求めた力に基づいて、トラベリングプレート、複数の
ロッド及び複数のアクチュエータのそれぞれに生ずる弾
性変形による変形量を求め、機構パラメータ修正手段に
より、弾性変形量取得手段により求めた弾性変形量に基
づいて、機構パラメータを修正し、第２位置情報検出手
段により、機構パラメータ修正手段により修正した機構
パラメータに基づいて、工具ユニットの目標位置に対応
する複数のアクチュエータの位置情報を求める。即ち、
目標位置及び姿勢に応じてトラベリングプレート、複数
のロッド及び複数のアクチュエータにそれぞれ加わる力
からそれぞれに生ずる弾性変形量に基づいて機構パラメ
ータが修正され、この修正した機構パラメータに基づい
て工具ユニットの目標位置に対応する複数のアクチュエ
ータの位置情報を求める。これにより、位置、姿勢の変
化によって各軸に分配される負荷バランスが変化して
も、このような負荷バランスの変化により生じるトラベ
リングプレート、複数のロッド及び複数のアクチュエー
タの弾性変形を考慮して工具ユニットの目標位置に対応
する複数のアクチュエータの位置情報を求めることがで
きる。

【００１９】

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

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

【００２１】基台１１は六角形状をなす部材であり、周
囲に３つの支持部１１ａが等間隔で設けられており、こ
の支持部１１ａにアクチュエータユニット２０を介して
ロッド１５が２本ずつ合計６本連結されている。

【００２２】アクチュエータユニット２０は、スライド
２２、スライダ２６、アクチュエータとしてのボールね
じ２４及びサーボモータ２５Ｕ、２５ｕ、２５Ｖ、２５
ｖ、２５Ｗ、２５ｗ（以下、これら６個のモータを総称
して「モータ２５Ｕ〜２５ｗ」という。）から構成され
ている。このモータ２５Ｕ〜２５ｗには、それぞれモー
タ位置検出用エンコーダ３１Ｕ、３１ｕ、３１Ｖ、３１
ｖ、３１Ｗ、３１ｗ（以下、これら６個のモータ位置検
出用エンコーダを総称して「エンコーダ３１Ｕ〜３１
ｗ」という。）が取り付けられている。

【００２３】スライド２２は断面形状がコ型をした部材
であり、それぞれのスライド２２は基台１１に対して所
定角度（例えば４５度）に傾斜して放射状に基台１１に
固定されている。これらのスライド２２にはその長手方
向にスライダ２６が摺動可能に支持されている。またス
ライド２２には、スライダ２６に螺合可能なボールねじ
２４が回動可能に支持されており、このボールねじ２４
の一旦側にそれぞれのスライダ２２に対応するモータ２
５Ｕ〜２５ｗが連結されている。これにより、ボールね
じ２４に連結されるモータ２５Ｕ〜２５ｗを駆動するこ
とにより、それぞれのボールねじ２４を回動させ、その
結果としてスライダ２６をスライド２２の長手方向に移
動させ得るようになっている。

【００２４】上述したスライダ２６には、例えば２個の
ベアリングジョイント、即ち第５ジョイント１６ａ及び
第４ジョイント１６ｂからなる「基台側ジョイント」を
介してそれぞれロッド１５一端側が連結される。これに
より、ロッド１５は「基台側ジョイント」によりスライ
ダ２６に対して３次元方向に揺動可能となっている。一
方、各ロッド１５の他端は、例えば３個のベアリングジ
ョイント、即ち第１ジョイント１７ａ、第２ジョイント
１７ｂ及び第３ジョイント１７ｃからなる「ＴＰ側ジョ
イント」を介してトラベリングプレート１２の同一平面
上に連結される。これにより、ロッド１５は「ＴＰ側ジ
ョイント」によりトラベリングプレート１２に対して３
次元方向に揺動可能となっている。

【００２５】トラベリングプレート１２は、基台１１と
同様に六角形状をなす部材であり、基台１１よりも小型
形状に形成されている。そして、その周囲には、基台１
１の支柱部１１ａに対応する位置に、ロッド１５の他端
側が２本ずつ合計６本連結されている。トラベリングプ
レート１２の下部には、ドリルやエンドミル等の工具Ｔ
を駆動する工具ユニットＵが取り付けられている。

【００２６】以上の構成により、後述するの制御装置よ
り動作指令を与えることによって、アクチュエータユニ
ット２０のモータ２５Ｕ〜２５ｗを個別に駆動して６本
のロッド１５をそれぞれ独立して揺動させる。すると、
この６本のロッド１５の揺動の組み合わせにより、トラ
ベリングプレート１２を６自由度制御（位置及び姿勢制
御）することができる。

【００２７】つまり、各支持部１１ａに支持された２本
１組のロッド１５を同期して３組のロッド１５を個別に
駆動することによりトラベリングプレート１２の位置を
決定し、各支持部１１ａに固定された２本１組のロッド
１５のうちの各１本、即ち合計３本のロッド１５を駆動
することによりトラベリングプレート１２の姿勢を決定
することができるようになっている。そして、トラベリ
ングプレート１２に取り付けられた工具Ｔを所望の位置
及び姿勢に移動し、工作物の加工を行うようになってい
る。ここで、トラベリングプレート１２及び工具ユニッ
トＵの合計質量は、例えば約２００Ｋｇである。

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

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

【００３０】次に、本実施形態に係るパラレルリンク機
構１０の制御方法を図４〜図１６に基づいて説明する。
なお、本実施形態に係る制御方法は、前述したパラレル
リンク機構１０においては、制御装置７０により行われ
るもので、本制御方法に基づく各処理を記述した制御プ
ログラムは、制御装置７０のメモリ７２等に記憶されて
いる。

【００３１】図４に示すように、本制御方法は、主に、
ステップＳ１０、Ｓ１２、Ｓ１４、Ｓ１６、Ｓ１８、Ｓ
２０、Ｓ２２、Ｓ２４、Ｓ２６からなるアルゴリスムに
より処理される。ここで、処理の概要を簡単に述べる
と、次のようになる。

【００３２】まず、機構パラメータを初期値に設定した
後（Ｓ１０）、無負荷状態、即ち弾性変形を無視した状
態で逆変換を行い、アクチュエータ座標を計算する（Ｓ
１２）。次に、計算したアクチュエータ座標からロッド
軸方向の単位ベクトル及び各ジョイントの角度を計算し
た上で（Ｓ１４、Ｓ１６）、各部材に加わる力を軸方向
及び曲げ方向に分解して求める（Ｓ１８）。

【００３３】ここで、求めた力と前回求めた力との差が
十分に小さいか否か、即ち許容誤差の範囲内にあるか否
かを判断する（Ｓ２０）。そして、許容誤差の範囲内に
あれば（Ｓ２０でＮｏ）、処理を終了し（ＥＮＤ）、そ
うでなければ（Ｓ２０でＹｅｓ）、力の差による各部材
の弾性変形量を計算し（Ｓ２２）、機構パラメータの値
を修正する（Ｓ２４）。修正後の機構パラメータは次の
Ｓ２６に用いられる。

【００３４】最後のステップＳ２６では、修正後の機構
パラメータで逆変換を行い、再びアクチュエータ座標を
計算する。そして、求めたアクチュエータ座標に基づい
て、再度、ステップＳ１４〜２６までの処理を繰り返
す。即ち、ステップＳ１８により求めた力の差が十分に
小さくなるまで、即ち許容誤差の範囲内に入るまで、収
束計算を繰り返すようにしている。これにより、ステッ
プＳ２２による弾性変形量をより正確に求めることがで
きるので、アクチュエータ座標の精度を高めることがで
きる。したがって、位置決め精度をさらに向上すること
ができる。

【００３５】なお、ステップＳ１８により求めた力と前
回求めた力との差は、修正後の機構パラメータによって
負荷の分担割合が変化して生じるものであるから、通常
は、１回目の計算値と２回目の計算値との差は極微小な
ものであると考えられる。したがって、前述したような
ステップＳ２６の終了後のループ処理（ステップＳ１４
〜２６までの処理の繰り返し）を行うことなく、ステッ
プＳ２６の終了後、処理を終了する（ＥＮＤ）ようにア
ルゴリスムを設定しても良い。これにより、処理の高速
化を図ることができる。

【００３６】続いて、上述した本実施形態の制御方法に
係る処理を各ステップごとに詳細に説明する。［１］ステップＳ１０ステップＳ１０では、機構パラメータＰに初期値Ｐ₀を
設定する処理が行われる。具体的には、次に説明する機
構パラメータＰに、予め決定されている所定値（Ｐ₀）
をセットする。

【００３７】［２］ステップＳ１２ステップＳ１２では、無負荷状態（ｆ₀＝０）で逆変換
を行い、アクチュエータ座標ｄ₀を計算する処理が行わ
れる。ここで、このような逆変換を行うのは、次のステ
ップＳ１４によりアクチュエータ軸方向等の力を計算す
る上で必要となるロッド１５の軸方向ベクトルを求める
ためである。また、逆変換は、通常、目標の工具Ｔの先
端座標に対するアクチュエータ座標を求めるために行わ
れ、具体的には、次の式(1) 〜式(5) に示すように計算
される。

【００３８】

【数１】

【数２】

【数３】

【数４】

【数５】ここで、上式(1) 、(3) 〜(5) に用いられるＲＬ、ＳＡ
１、ＳＡ２、ＢＯ１、ＢＯ２、ＢＯ３、ＳＬ、ＳＯ、Ｊ
Ｏ、ＴＯ、ＴＰＯ１、ＴＰＯ２、ＴＰＯ３は、図５に示
すように、機械の幾何学的な寸法を表すパラメータで、
「機構パラメータ」と呼ばれるものである。また式(2)
に用いられるｘ、ｙ、ｚは図５、図６に示す直交座標系
における位置を表し、Ａ、Ｂ、Ｃは、ｘ、ｙ、ｚ軸回り
の回転を表すものである。

【００３９】例えば、図５及び図６に示すＵ軸について
説明する。ＢＯ１、ＢＯ２、ＢＯ３は、スライド２２の
起点から基台１１の中心までのｘ、ｙ、ｚ方向の距離
（ベースオフセット）を表す（図５(A) 、図５(B) 、図
６(A) 参照）。またＴＰＯ１、ＴＰＯ２、ＴＰＯ３は、
トラベリングプレート１２とロッド１５とを連結する第
１ジョイント１７ａおよび第２ジョイント１７ｂからト
ラベリングプレート１２の中心までのｘ、ｙ、ｚ方向の
距離（ＴＰオフセット）を表す（図５(A) 、図５(C) 、
図６(B) 参照）。

【００４０】また、ＳＡ１は、スライド２２とｘ−ｙ平
面とが成す角度（スライダ取付角度）、ＳＡ２は、スラ
イド２２とｘ−ｚ平面とが成す角度（スライダ取付角
度）を表す（図５(A) 、図５(B) 、図６(A) 参照）。但
し、Ｖ（ｖ）軸、Ｗ（ｗ）軸では、それぞれ１２０゜、
２４０゜回転した座標系から見た距離及び角度となる。

【００４１】さらに、ＲＬはロッド１５の長さ（ロッド
長）、ＪＯはスライド２２とロッド１５とを接続するジ
ョイントのボールねじ２４の中心からのオフセット値
（ジョイントオフセット）、ＳＯはアクチュエータ軸の
原点の位置（スライダ原点）、ＳＬはスライド２２の長
さ（スライダ長）、ＴＯはトラベリングプレート１２か
らの工具Ｔの先端までの突出量（ＴＰオフセット）をそ
れぞれ表す（図５(A) 参照）。このようにＵ軸について
各パラメータが定義されるが、他の各軸（ｕ軸、Ｖ軸、
ｖ軸、Ｗ軸、ｗ軸）についてもＵ軸と同様に各パラメー
タが定義される。

【００４２】［３］ステップＳ１４ステップＳ１４では、アクチュエータ軸方向、ロッド方
向の力を計算する処理が行われる。このような処理を行
うのは、スライダ２６以下の全質量に対して、ロッド方
向にどのような力を発生させるか、またこのような力を
アクチュエータ軸方向に発生させるために必要なモータ
２５Ｕ〜２５ｗの駆動力を計算するためである。ここで
は、図７及び図８に示すような負荷質量の釣り合いモデ
ルに基づいて計算を行う。弾性変形は、パラレルリンク
機構１０を構成する各部材の自重によって生じ、位置及
び姿勢の変化による各軸負荷バランスの変化から変形量
が変化するからである。

【００４３】具体的には、図７及び図８に示すように、
スライダ２６以下の全質量を、基台側ジョイント（第５
ジョイント１６ａ及び第４ジョイント１６ｂ）の質量ｍ
_SJ、ロッド１５の質量ｍ_R、トラベリングプレート１２
の質量ｍ_Tに分けて考える。即ち、基台側ジョイントの
質量ｍ_SJは、スライド２２によるスライド面が支持す
る。またロッド１５の重心がその中央にある場合、ロッ
ド１５の質量ｍ_Rをその両端で支えているとすれば、そ
の半分はスライド面によって支持され、残りの半分はト
ラベリングプレート１２によって支持される。そのた
め、これらの質量とモータ２５が発生するボールねじ２
４の軸方向の力ｆ_s（＝［ｆ_SU ｆ_Su ｆ_SV ｆ_Sv ｆ_SW
ｆ_Sw］^T）の釣り合いを考える。

【００４４】まず、トラベリングプレート１２の重心ま
わりの釣り合いを考えると、６本のロッド１５の軸方向
の力ｆ_R（＝［ｆ_RU ｆ_Ru ｆ_RV ｆ_Rv ｆ_RW ｆ_Rw］^T）
の釣り合いから、次式(6) に示す関係が成り立つ。

【００４５】

【数６】ここで、上式(6) 中、ａ_RU、ａ_Ru、ａ_RV、ａ_Rv、ａ_RW、
ａ_Rwは、ロッド１５の軸方向単位ベクトルを示す。

【００４６】また、トラベリングプレート１２の重心か
ら各ＴＰ側ジョイント（第１ジョイント１７ａ、第２ジ
ョイント１７ｂ）までのベクトルをｂ_U、ｂ_u、ｂ_V、
ｂ_v、ｂ_W、ｂ_wとすると、モーメントの釣り合いによ
り、次式(7) が成り立つ。

【００４７】

【数７】但し、Ｏ＝［０００］^Tである。

【００４８】また、ロッド１５の軸方向単位ベクトルは
次式(8) 、トラベリングプレート１２の重心から各ＴＰ
側ジョイントまでのベクトルは次式(9) で表せられる。

【００４９】

【数８】

【数９】

【００５０】したがって、上式(6) 及び(7) は、次式(1
0)としてまとめることができる。

【００５１】

【数１０】

【００５２】そして、この式(10)を簡略化して式(11)に
より表すと、６本のロッド１５の軸方向の力ｆ_Rは、次
式(12)に示すように求めることができる。

【００５３】

【数１１】

【数１２】

【００５４】この力ｆ_Rのスライド２２の軸方向の分力
ｆ_S1（＝［ｆ_SU1 ｆ_Su1 ｆ_SV1 ｆ_Sv ₁ ｆ_SW1 ｆ_Sw1］^T）
は、スライド２２の軸方向単位ベクトルａ_Sとの内積か
ら求められ、Ｕ軸では次式(13)により与えられる。他の
各軸（ｕ軸、Ｖ軸、ｖ軸、Ｗ軸、ｗ軸）についてもＵ軸
と同様に与えられる。

【００５５】

【数１３】

【００５６】また、基台側ジョイント（第５ジョイント
１６ａ、第４ジョイント１６ｂ）とロッド１５の半分の
質量による重量ｍ_SJｇ、ｍ_Rｇ／２は、スライド面に対
して常に重力方向の力として作用するので、この力のス
ライド２２の軸方向の分力ｆ _S2（＝［ｆ_SU2 ｆ_Su2 ｆ
_SV2 ｆ_Sv2 ｆ_SW2 ｆ_Sw2］^T）は、Ｕ軸では、次式(14)に
示すように表される。

【００５７】

【数１４】

【００５８】以上から、ロッド１５及びロッド１５の両
端のジョイント（基台側ジョイント、ＴＰ側ジョイン
ト）には力ｆ_Rが働き、またボールねじ２４にはｆ_S＝
ｆ_S1＋ｆ_S2が働くことになる。

【００５９】［４］ステップＳ１６ステップＳ１６では、各ジョイント角度を計算する処理
が行われる。次のステップＳ１８による処理に各ジョイ
ント角度（特にθ₃、θ₅）が必要になるからである。
具体的には、同次変換行列表現を利用して求める。ここ
では、図９及び図１０に示すような４×４同次変換行列
（Ｄ−Ｈパラメータ）による座標系を定義する。図９に
示すＯ_i（i＝０〜６）は、第ｉ座標系の原点を示す。
またＯ₀は、工具Ｔの先端座標系であり、ワールド座標
系Ｏ_Wに工具Ｔの先端座標（ｘ、ｙ、ｚ、Ａ、Ｂ、Ｃ）
だけ並進、傾斜した座標系になる。

【００６０】次に、同次変換行列の規則に則って次のよ
うにＤ−Ｈパラメータを定義する。ここで、同次変換行
列の規則とは、次の〜に定められるものである。Ｚ_i-1軸を中心としたｘ_i-1軸からｘ_i軸までの回転
数をθ_iとする。回転中心を左にみて回る方向を正にと
る。第_i-1座標系の原点Ｏ_i-1からｚ_i-1軸上のｚ_i-1軸
とｘ_i軸との交点までの距離をｄ_iとする。Ｚ_i-1軸とｘ_i軸との交点から、Ｏ_iまでの距離をａ
_iで表す。ｘ_i軸を中心にしてｚ_i-1軸からｚ_i軸までの角度を
α_iで表す。回転中心を左にみる方向を正にとる。

【００６１】以下、図１１〜図１４に基づいて、具体的
なＤ−Ｈパラメータを定義する。i-1 座標系とｉ座標系
との関係は、それぞれの座標系における位置ベクトルを
ｘ_i-1、ｘ_iとすると、Ｄ−Ｈパラメータ（θ_i、
ｄ_i、ａ_i、α_i）を用いて、次の式(15)〜式(17)に示
すように表現することができる。

【００６２】

【数１５】

【数１６】

【数１７】

【００６３】また、式(17)に示す行列式中で回転を表す
部分、即ち破線に囲まれる部分を取り出しＲ_iとする
と、次式(18)に示すように表すことができる。

【００６４】

【数１８】

【００６５】そして、このＲ_iから、第ｉ座標系におけ
る方向ベクトルｑ_iの第i-1 座標系からみた方向ベクト
ルｑ_iー1を次式(19)のように求めることができる。な
お、この変換は、回転のみの変換であり、並進方向の変
換は伴わないものとする。

【００６６】

【数１９】

【００６７】この式(19)に示す関係をジョイント角度の
計算等に使用する。ここで、図１１(A) 、(B) には、ト
ラベリングプレート１２と第１ジョイント１７ａとの関
係が示されており、Ｄ−Ｈパラメータ（θ₁、ｄ₁、ａ
₁、α₁）が定義されている。また図１２(A) には、第
１ジョイント１７ａと第２ジョイント１７ｂとの関係が
示されており、Ｄ−Ｈパラメータ（θ₂、ｄ₂、ａ₂、
α₂）が定義されている。さらに図１２(B) には、第２
ジョイント１７ｂと第３ジョイント１７ｃとの関係が示
されており、Ｄ−Ｈパラメータ（θ₃、ｄ₃、ａ₃、α
₃）が定義されている。

【００６８】また、図１３(A) には、第３ジョイント１
７ｃと第４ジョイント１６ｂとの関係が示されており、
Ｄ−Ｈパラメータ（θ₄、ｄ₄、ａ₄、α₄）が定義さ
れている。図１３(B) には、第４ジョイント１６ｂと第
５ジョイント１６ａとの関係が示されており、Ｄ−Ｈパ
ラメータ（θ₅、ｄ₅、ａ₅、α₅）が定義されてい
る。図１４(A) には、第５ジョイント１６ａとスライダ
２６との関係が示されており、Ｄ−Ｈパラメータ
（θ₆、ｄ₆、ａ₆、α₆）が定義されている。

【００６９】ここで、上述した各ジョイントのＤ−Ｈパ
ラメータ（θ_i、ｄ_i、ａ_i、α_i（i＝１〜６））を整
理すると、次表に示すようにまとめられる。

【００７０】

【表１】

【００７１】次に、このＤ−Ｈパラメータを用いて、各
ジョイント角度θ₂〜θ₅を順次、求める。ここで、図
１４(B) に示すように、ワールド座標系におけるロッド
１５方向の単位ベクトルａ_Rと第０座標系での同ベクト
ルａ_R0との関係は、次の式(20)、式(21)に示すように表
される。

【００７２】

【数２０】

【数２１】

【００７３】したがって、ａ_R0は、次式(22)に示すよう
に求めることができる。

【００７４】

【数２２】

【００７５】また、第１座標系における同ベクトルは、
次の式(23)〜式(25)に示すように求めることができる。

【００７６】

【数２３】

【数２４】

【数２５】

【００７７】ここで、Ｒ₀は、ワールド座標に対する工
具Ｔの先端の回転を表す行列であり、Ｒ₁は、前述した
ように、式(17)の同次変換行列の回転部分を取り出した
もの（式(18)）で、第１座標系の第０座標系に対する回
転を表す３×３行列である。また、ａ_R1を第１座標系の
ｘ₁−ｙ₁平面に射影したベクトルが、ｘ₁軸と成す角
度がθ₂になる。したがって、θ₂は、次式(26)、(27)
に示すように求めることができる。

【００７８】

【数２６】

【数２７】

【００７９】同様に、θ₃は、第２座標系におけるロッ
ド方向ベクトルａ_R2を第２座標系のｘ₂−ｙ₂平面に射
影したベクトルとｘ₂軸と成す角度になるので、次式(2
8)〜式(31)に示すように求めることができる。

【００８０】

【数２８】

【数２９】

【数３０】

【数３１】

【００８１】次に、θ₄は、第３座標系のｙ₃軸と第４
座標系のｚ₄軸の成す角度であると考えることができ
る。そこで、まずｚ₄軸方向ベクトルｕ_Z4をワールド座
標系で求める。そして、ｚ₄軸ベクトルは、ロッド方向
単位ベクトルａ_Rとスライド面へのアプローチベクトル
Ａ_S（ｚ₄軸と同方向のベクトル）の両者に直交し、か
つ、大きさが１のベクトルであることから、次式(32)に
示す連立方程式を解くことにより求めることができる。

【００８２】

【数３２】

【００８３】そして、ｕ_Z4の第３座標系でのベクトルｕ
_{Z4_3}と、ｙ₃軸とが成す角度として次式(33)〜(36)に示
すように、θ₄を求めることができる。

【００８４】

【数３３】

【数３４】

【数３５】

【数３６】

【００８５】最後に、θ₅をスライドアプローチベクト
ルＡｓとロッド方向ベクトルａ_Rとの成す角度として、
次式(37)に示すように求めることができる。

【００８６】

【数３７】

【００８７】［５］ステップＳ１８ステップＳ１８では、各部材の力のかかる方向に、力を
分解する処理が行われる。具体的には、先に求めたジョ
イント角度θ₃、θ₅を用いてロッド方向の力を第１ジ
ョイント１７ａおよび第５ジョイント１６ａのそれぞれ
の回転軸のアキシャル方向、曲げ方向の力に分解する。
第１ジョイント１７ａの回転軸のアキシャル方向の力ｆ
_1Aおよび曲げ方向の力ｆ_1Mは、それぞれ次の式(38)、式
(39)に示すように表される。

【００８８】

【数３８】

【数３９】

【００８９】また、第５ジョイント１６ａの回転軸のア
キシャル方向の力ｆ_1Aおよび曲げ方向の力ｆ_1Mは、それ
ぞれ次の式(40)、式(41)に示すように表される。

【００９０】

【数４０】

【数４１】

【００９１】［６］ステップＳ２０ステップＳ２０では、収束判定処理が行われる。具体的
には、前のステップＳ１８により求めた力ｆ_iと前回求
めた力ｆ_i-1との差が十分に小さいか否かを判断する処
理が行われる。このような収束判定処理が行われるの
は、一般的に、機構パラメータが変われば、力の釣り合
いも変化するため、弾性変形量を計算して機構パラメー
タを修正するような計算をする場合、これを繰り返す収
束計算を行わなければ、正確な値に到達し難いからであ
る。なお、この判定定数εは、力の差によって生じ得る
工具Ｔの先端位置誤差が、その許容誤差範囲内に収まる
ように設定されている。

【００９２】［７］ステップＳ２２ステップＳ２２では、各部材の弾性変形量を計算する処
理が行われる。具体的には、前のステップＳ１８で求め
た各部にかかる力と、既知のコンプライアンスとを用い
て各部の変形量を計算する（図１５参照）。まず、ロッ
ド１５の軸方向変位を次式(42)により求める。

【００９３】

【数４２】

【００９４】次にＴＰ側ジョイント位置の変位を次式(4
3)により求める。

【００９５】

【数４３】ここで、上式(42)、(43)において、Ｃ_Rはロッド１５の
パイプ部におけるアキシャル方向コンプライアンス、Ｃ
_Hは第１ジョイント１７ａ、第５ジョイント１６ａの回
転軸の曲げ方向コンプライアンス、Ｃ_JxAはジョイント
ｘのアキシャル方向コンプライアンス、Ｃ_JxRはジョイ
ントｘのラジアル方向コンプライアンス、Ｃ_JxMはジョ
イントｘの曲げ方向コンプライアンス、Ｃ_Bはボールね
じ２４の軸方向コンプライアンスをそれぞれ表すパラメ
ータである。

【００９６】そして、これを工具先端座標系における変
位に変換すると、次の式(44)、式(45)に示すように表さ
れる。

【００９７】

【数４４】

【数４５】

【００９８】次に基台側ジョイント位置の変位を求め
る。具体的には、まず第５座標系における変位を次式(4
6)計算する

【００９９】

【数４６】

【０１００】これをワールド座標系における変位に変換
すると、次の式(47)、式(48)に示すように表される。

【０１０１】

【数４７】

【数４８】

【０１０２】次にボールねじ２４の変位を求める。具体
的には次式(49)に示すように求められる。なお、実際に
は、アクチュエータ軸原点のずれとしてパラメータの修
正がなされる。

【０１０３】

【数４９】

【０１０４】［８］ステップＳ２４ステップＳ２４では、各機構パラメータの修正をする処
理が行われる。つまり、上述した式(42)〜式(49)の結果
に基づき機構パラメータを修正する。まず、次式(50)に
より、ロッド長の修正が行われる。

【０１０５】

【数５０】

【０１０６】次にＴＰ側ジョイント位置の修正が行われ
る。Ｕ軸の場合には次の式(51)に示すように修正され
る。

【０１０７】

【数５１】

【０１０８】次に基台側ジョイント位置の修正が行われ
る。これは、図１６に示すように、等価的にスライド２
２の位置を修正することによっても可能である。例えば
スライド２２の一端側の位置（AE1x、AE1y、AE1z）、他
端側の位置（AE2x、AE2y、AE2z）とすると、Ｕ軸の場合
には次の式(52)及び式(53)から式(54)に示すように修正
される。

【０１０９】

【数５２】

【数５３】

【数５４】

【０１１０】次にボールねじ２４の変位をアクチュエー
タ軸原点ＳＯの変位として次式(53)に示すように修正が
行われる。

【０１１１】

【数５５】

【０１１２】［９］ステップＳ２６ステップＳ２６では、修正した機構パラメータを用いた
逆変換が行われる。つまり、上述した式(50)〜式(56)に
より修正された機構パラメータを用いて、ステップＳ１
２と同様、式(1) による逆変換の処理を行う。これによ
って修正後の機構パラメータにより、アクチュエータ座
標ｄ_iを計算する処理が行われる。

【０１１３】次に、本実施形態に係る制御方法に基づい
てパラレルリンク機構１０の制御を行った実験結果を図
１７を参照して説明する。図１７に示す実験結果は、パ
ラレルリンク機構１０のｚ軸を一定にし、Ａ軸（同図
(A) ）およびＢ軸（同図(B) ）の傾き角を−３０゜〜＋
３０゜に変化させ、工具Ｔによりワークを加工したもの
である。なお、本実施形態に係る制御方法に係るものは
白丸（○）によりプロットした特性曲線により、また従
来の制御によるものは黒丸（●）によりプロットした特
性曲線により、それぞれ図７(A) 、(B) に示されてい
る。

【０１１４】この結果、本実施形態に係る制御に係る弾
性変形を考慮したものは（○）、Ａ軸及びＢ軸の回転角
度が大きくてもｚ方向位置誤差が０．００mm±０．１０
mm前後の範囲内に収まっているのに対し、従来の制御に
よるものは（●）、傾き角が大きくなるにつれて位置誤
差が本実施形態に係る制御のものよりも大きくなってい
ることがわかる。この実験結果により、本実施形態に係
る制御方法によるものでは、工具先端位置の位置決め精
度を向上し得ることを確認した。

【０１１５】以上説明したように、本実施形態に係るパ
ラレルリンク機構の制御方法によると、ステップＳ１
０、Ｓ１２により、所定の機構パラメータＰ₀に基づい
て工具ユニットＵの目標位置に対応する複数のボールね
じ２４（アクチュエータ）のアクチュエータ座標ｄ₀を
求め、ステップＳ１４、Ｓ１６、Ｓ１８により、複数の
ボールねじ２４（アクチュエータ）のアクチュエータ座
標ｄ₀に基づいて、目標位置及び姿勢に応じてトラベリ
ングプレート１２、複数のロッド１５及び複数のボール
ねじ２４にそれぞれ加わる力を求め、ステップＳ２２に
より、求めた力に基づいて、トラベリングプレート１
２、複数のロッド１５及び複数のボールねじ２４のそれ
ぞれに生ずる弾性変形による変形量を求め、ステップＳ
２４により、弾性変形量に基づいて、機構パラメータＰ
を修正し、ステップＳ２６により、修正した機構パラメ
ータＰに基づいて、工具ユニットＵの目標位置に対応す
る複数のボールねじ２４（アクチュエータ）のアクチュ
エータ座標ｄ₁を求める。

【０１１６】即ち、目標位置及び姿勢に応じてトラベリ
ングプレート１２、複数のロッド１５及び複数のボール
ねじ２４（アクチュエータ）にそれぞれ加わる力からそ
れぞれに生ずる弾性変形量に基づいて機構パラメータＰ
が修正され、この修正した機構パラメータＰに基づいて
工具ユニットＵの目標位置に対応する複数のボールねじ
２４（アクチュエータ）のアクチュエータ座標を求め
る。これにより、位置、姿勢の変化によって各軸に分配
される負荷バランスが変化しても、このような負荷バラ
ンスの変化により生じるトラベリングプレート１２、複
数のロッド１５及び複数のボールねじ２４（アクチュエ
ータ）の弾性変形を考慮して工具ユニットＵの目標位置
に対応する複数のボールねじ２４（アクチュエータ）の
アクチュエータ座標を求めることができる。したがっ
て、かかる弾性変形による制御性能の低下を抑制できる
ため、位置決め精度を向上し得る効果がある。

【０１１７】さらに本実施形態では、複数のロッド１５
及び複数のボールねじ２４以外にも、複数のジョイント
のそれぞれに加わる力を算出し、弾性変形量を求めるよ
うにしているので、さらに位置決め精度を向上すること
ができる。

【０１１８】なお、本実施形態では、工具ユニットとし
て、ドリルやエンドミル等の工具を取り付けたが、これ
ら切削に用いる工具だけでなく、砥石等の研削工具、レ
ーザトーチ、ロボットハンド、各種測定機器等を取り付
けることができる。

【０１１９】

【発明の効果】請求項１の発明では、第１ステップによ
り、所定の機構パラメータに基づいて工具ユニットの目
標位置に対応する複数のアクチュエータの位置情報を求
め、第２ステップにより、第１ステップにより求めた複
数のアクチュエータの位置情報に基づいて、目標位置及
び姿勢に応じてトラベリングプレート、複数のロッド及
び複数のアクチュエータにそれぞれ加わる力を求め、第
３ステップにより、第２ステップにより求めた力に基づ
いて、トラベリングプレート、複数のロッド及び複数の
アクチュエータのそれぞれに生ずる弾性変形による変形
量を求め、第４ステップにより、第３ステップにより求
めた弾性変形量に基づいて、機構パラメータを修正し、
第５ステップにより、第４ステップにより修正した機構
パラメータに基づいて、工具ユニットの目標位置に対応
する複数のアクチュエータの位置情報を求める。即ち、
目標位置及び姿勢に応じてトラベリングプレート、複数
のロッド及び複数のアクチュエータにそれぞれ加わる力
からそれぞれに生ずる弾性変形量に基づいて機構パラメ
ータが修正され、この修正した機構パラメータに基づい
て工具ユニットの目標位置に対応する複数のアクチュエ
ータの位置情報を求める。これにより、位置、姿勢の変
化によって各軸に分配される負荷バランスが変化して
も、このような負荷バランスの変化により生じるトラベ
リングプレート、複数のロッド及び複数のアクチュエー
タの弾性変形を考慮して工具ユニットの目標位置に対応
する複数のアクチュエータの位置情報を求めることがで
きる。したがって、かかる弾性変形による制御性能の低
下を抑制できるため、位置決め精度を向上し得る効果が
ある。

【０１２０】請求項２の発明では、第３ステップは、目
標位置及び姿勢に応じて、基台側ジョイントおよびＴＰ
側ジョイントにそれぞれ加わる力に基づいて、基台側ジ
ョイント及びＴＰ側ジョイントに生ずる弾性変形による
変形量も求める。これにより、基台と前記ロッドとの間
には、回転軸の方向が異なる複数の基台側ジョイントが
介在し、またトラベリングプレートとロッドとの間に
は、回転軸の方向が異なる複数のＴＰ側ジョイントが介
在した場合において、位置、姿勢の変化によって各軸に
分配される負荷バランスが変化しても、このような負荷
バランスの変化により生じる基台側ジョイントおよびＴ
Ｐ側ジョイントの弾性変形を考慮して工具ユニットの目
標位置に対応する複数のアクチュエータの位置情報を求
めることができる。したがって、かかる基台側ジョイン
ト及びＴＰ側ジョイントの弾性変形による制御性能の低
下を抑制できるため、位置決め精度を向上し得る効果が
ある。

【０１２１】請求項３の発明では、第２ステップにより
求めた力と同ステップにより前回求めた力との差が、所
定値よりも小さくなるまで、第２ステップ、第３ステッ
プ、第４ステップ及び第５ステップによる各処理を繰り
返し行う。即ち、所定値よりも小さくなるまで、第２ス
テップ、第３ステップ、第４ステップ及び第５ステップ
による収束計算を行うことができる。これにより、弾性
変形量をより正確に求めることができるので、複数のア
クチュエータの位置情報の精度を高めることができる。
したがって、位置決め精度をさらに向上し得る効果があ
る。

【０１２２】請求項４の発明では、第１位置情報取得手
段により、所定の機構パラメータに基づいて工具ユニッ
トの目標位置に対応する複数のアクチュエータの位置情
報を求め、負荷取得手段により、位置情報取得手段によ
り求めた複数のアクチュエータの位置情報に基づいて、
目標位置及び姿勢に応じてトラベリングプレート、複数
のロッド及び複数のアクチュエータにそれぞれ加わる力
を求め、弾性変形量取得手段により、負荷取得手段によ
り求めた力に基づいて、トラベリングプレート、複数の
ロッド及び複数のアクチュエータのそれぞれに生ずる弾
性変形による変形量を求め、機構パラメータ修正手段に
より、弾性変形量取得手段により求めた弾性変形量に基
づいて、機構パラメータを修正し、第２位置情報検出手
段により、機構パラメータ修正手段により修正した機構
パラメータに基づいて、工具ユニットの目標位置に対応
する複数のアクチュエータの位置情報を求める。即ち、
目標位置及び姿勢に応じてトラベリングプレート、複数
のロッド及び複数のアクチュエータにそれぞれ加わる力
からそれぞれに生ずる弾性変形量に基づいて機構パラメ
ータが修正され、この修正した機構パラメータに基づい
て工具ユニットの目標位置に対応する複数のアクチュエ
ータの位置情報を求める。これにより、位置、姿勢の変
化によって各軸に分配される負荷バランスが変化して
も、このような負荷バランスの変化により生じるトラベ
リングプレート、複数のロッド及び複数のアクチュエー
タの弾性変形を考慮して工具ユニットの目標位置に対応
する複数のアクチュエータの位置情報を求めることがで
きる。したがって、かかる弾性変形による制御性能の低
下を抑制できるため、位置決め精度を向上し得る効果が
ある。

【図面の簡単な説明】

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

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

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

【図４】本実施形態に係る制御方法による手順の流れを
示すフローチャートである。

【図５】本実施形態に係る制御方法に用いられる各機構
パラメータを示す説明図で、図５(A) は機構パラメータ
ＳＡ１、ＲＬ、ＢＯ１、ＴＯ、ＳＯ、ＪＯを明示するも
の、図５(B) は機構パラメータＢＯ３を明示するもの、
図５(C) はＴＰＯ３を明示するものである。

【図６】本実施形態に係る制御方法に用いられる各機構
パラメータを示す説明図で、図６(A) は機構パラメータ
ＳＡ２、ＢＯ２を明示するもの、図６(B) は機構パラメ
ータＴＰＯ１、ＴＰＯ２を明示するものである。

【図７】本実施形態の制御方法によるステップＳ１４に
用いられる負荷質量の釣り合いモデルを示す説明図であ
る。

【図８】本実施形態の制御方法によるステップＳ１４に
用いられる負荷質量の釣り合いモデルを示す説明図であ
る。

【図９】本実施形態の制御方法によるステップＳ１６に
用いられる４×４同次変換行列（Ｄ−Ｈパラメータ）表
現のための座標系定義を、トラベリングプレート等を含
めて表した説明図である。

【図１０】図９からトラベリングプレート等を除いてＤ
−Ｈパラメータの座標系定義を示した説明図である。

【図１１】図９に示すＤ−Ｈパラメータの座標系定義を
各ジョイントごとに分解した説明図で、図１１(A) はト
ラベリングプレートと第１ジョイントとの関係を示すも
の、図１１(B) は図１１(A) をｙ_T軸方向から見たもの
である。

【図１２】図９に示すＤ−Ｈパラメータの座標系定義を
各ジョイントごとに分解した説明図で、図１２(A) は第
１ジョイントと第２ジョイントとの関係を示すもの、図
１２(B) は第２ジョイントと第３ジョイントとの関係を
示すものである。

【図１３】図９に示すＤ−Ｈパラメータの座標系定義を
各ジョイントごとに分解した説明図で、図１３(A) は第
３ジョイントと第４ジョイントとの関係を示すもの、図
１３(B) は第４ジョイントと第５ジョイントとの関係を
示すものである。

【図１４】図１４(A) は、図９に示すＤ−Ｈパラメータ
の座標系定義を各ジョイントごとに分解した説明図で第
５ジョイントとスライダとの関係を示すもの、図１４
(B)は、ロッド方向のベクトルを示す説明図である。

【図１５】本実施形態の制御方法によるステップＳ２２
に用いられる各部に加わる力の方向を示す説明図であ
る。

【図１６】本実施形態の制御方法による「各機構パラメ
ータの修正」に用いられる第４、第５ジョイントの変位
の様子を示す説明図である。

【図１７】パラレルリンク機構によりｚ軸を一定、傾き
角を変化させた場合におけるワークの加工誤差（ｚ軸方
向位置誤差）を示す特性図で、図１７(A) はＡ軸の回転
角度に対するもの、図１７(B) はＢ軸の回転角度に対す
るものである。

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

【図１９】パラレルリンク機構を原点位置で、Ａ軸の角
度を変化させたときのロッド軸方向の負荷変化を示す特
性図である。

【符号の説明】

１０パラレルリンク機構１１基台１２トラベリングプレート１５ロッド１６ａ第５ジョイント（基台側ジョイント）１６ｂ第４ジョイント（基台側ジョイント）１７ａ第１ジョイント（ＴＰ側ジョイント）１７ｂ第２ジョイント（ＴＰ側ジョイント）１７ｃ第３ジョイント（ＴＰ側ジョイント）２２スライド２４ボールねじ（アクチュエータ）２５Ｕ〜２５ｗモータ（サーボユニット）２６スライダ３１Ｕ〜３１ｗエンコーダ７０制御装置（第１位置情報取得手段、負
荷取得手段、弾性変形量取得手段、機構パラメータ修正
手段、第２位置情報取得手段）Ｔ工具Ｕ工具ユニット ε 所定値Ｓ１０、Ｓ１２第１ステップＳ１４、Ｓ１６、Ｓ１８第２ステップＳ２２第３ステップＳ２４第４ステップＳ２６第５ステップ

Claims

【特許請求の範囲】

【請求項１】外部に固定される基台、工具ユニットの
装着されるトラベリングプレート、前記基台と前記トラ
ベリングプレートを連結する複数のロッド、前記複数の
ロッドを駆動する複数のアクチュエータ、前記各アクチ
ュエータを制御する複数のサーボユニット及び前記サー
ボユニットへ指令を与え前記アクチュエータを制御する
制御装置、を備えたパラレルリンク機構の制御方法であ
って、所定の機構パラメータに基づいて前記工具ユニットの目
標位置に対応する前記複数のアクチュエータの位置情報
を求める第１ステップと、前記第１ステップにより求めた複数のアクチュエータの
位置情報に基づいて、目標位置及び姿勢に応じて前記ト
ラベリングプレート、前記複数のロッド及び前記複数の
アクチュエータにそれぞれ加わる力を求める第２ステッ
プと、前記第２ステップによる求めた力に基づいて、前記トラ
ベリングプレート、前記複数のロッド及び前記複数のア
クチュエータのそれぞれに生ずる弾性変形による変形量
を求める第３ステップと、前記第３ステップにより求めた弾性変形量に基づいて、
前記機構パラメータを修正する第４ステップと、前記第４ステップにより修正した機構パラメータに基づ
いて、前記工具ユニットの目標位置に対応する前記複数
のアクチュエータの位置情報を求める第５ステップと、
を含むことを特徴とするパラレルリンク機構の制御方
法。
【請求項２】前記基台と前記ロッドとの間には、回転
軸の方向が異なる複数の基台側ジョイントが介在し、ま
た前記トラベリングプレートと前記ロッドとの間には、
回転軸の方向が異なる複数のＴＰ側ジョイントが介在す
る請求項１記載のパラレルリンク機構の制御方法であっ
て、前記第３ステップは、目標位置及び姿勢に応じて、前記
基台側ジョイントおよび前記ＴＰ側ジョイントにそれぞ
れ加わる力に基づいて、前記基台側ジョイント及びＴＰ
側ジョイントに生ずる弾性変形による変形量も求めるこ
とを特徴とする請求項１記載のパラレルリンク機構の制
御方法。
【請求項３】前記第２ステップにより求めた力と同ス
テップにより前回求めた力との差が、所定値よりも小さ
くなるまで、前記第２ステップ、第３ステップ、第４ス
テップ及び第５ステップによる各処理を繰り返し行うこ
とを特徴とする請求項１または２記載のパラレルリンク
機構の制御方法。
【請求項４】外部に固定される基台、工具ユニットの
装着されるトラベリングプレート、前記基台と前記トラ
ベリングプレートを連結する複数のロッド、前記複数の
ロッドを駆動する複数のアクチュエータ及び前記各アク
チュエータを制御する複数のサーボユニット、を有する
パラレルリンク機構の前記サーボユニットに指令を与え
前記アクチュエータを制御するパラレルリンク機構の制
御装置であって、所定の機構パラメータに基づいて前記工具ユニットの目
標位置に対応する前記複数のアクチュエータの位置情報
を求める第１位置情報取得手段と、前記位置情報取得手段により求めた複数のアクチュエー
タの位置情報に基づいて、目標位置及び姿勢に応じて前
記トラベリングプレート、前記複数のロッド及び前記複
数のアクチュエータにそれぞれ加わる力を求める負荷取
得手段と、前記負荷取得手段による求めた力に基づいて、前記トラ
ベリングプレート、前記複数のロッド及び前記複数のア
クチュエータのそれぞれに生ずる弾性変形による変形量
を求める弾性変形量取得手段と、前記弾性変形量取得手段により求めた弾性変形量に基づ
いて、前記機構パラメータを修正する機構パラメータ修
正手段と、前記機構パラメータ修正手段により修正した機構パラメ
ータに基づいて、前記工具ユニットの目標位置に対応す
る前記複数のアクチュエータの位置情報を求める第２位
置情報検出手段と、とを備えることを特徴とするパラレルリンク機構の制御
装置。