JPH11333771A - 冗長運動機構の制御方法およびその制御装置並びに冗長運動機構および２自由度並進駆動機構 - Google Patents

Abstract

(57)【要約】 【課題】マクロ／マイクロ機構から構成された冗長運動
機構において、工具の運動として一つのコンプライアン
ス特性モデルを与えることによりコンプライアンス制御
を実現し、広い動作領域で高い応答周波数特性を要求さ
れる作業を可能にした冗長運動機構の制御方法およびそ
の制御装置を提供することにある。 【解決手段】本発明は、マクロ機構（ロボット）１０の
手先に応答周波数の高い微動機構２０をつけたマクロ／
マイクロ機構において、工具の経路情報とコンプライア
ンス制御式から工具の合成運動指令値を算出し、この指
令値のうち、微動機構と冗長な運動成分と冗長でない運
動成分とに分解し、この分解された冗長な運動成分の指
令値をローパスフィルターに通して抽出した低周波数成
分と前記冗長でない運動成分の指令値とを合成したもの
をマクロ機構の運動指令値とし、微動機構へは、冗長な
運動成分の指令値から、マクロ機構の出力運動と動作範
囲の特定点からの位置偏差に定数を掛けた値とを差し引
いた値を、指令値として与えることを特徴とする。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、鋳物品のバリ取り
や表面磨き作業、タービンブレードやポンプの翼の機械
加工後のグラインダーによる磨き作業、アーク溶接後の
溶接ビード磨き作業などの仕上げ作業や、組立作業にお
けるはめ合い動作等の、位置と力の制御を必要とする作
業の自動制御に適用する冗長運動機構の制御方法および
その制御装置並びに冗長運動機構および２自由度並進駆
動機構に関する。

【０００２】

【従来の技術】磨き作業では、ＣＡＤデータから導いた
ロボットの軌道と実際のワークの設置位置や形状に誤差
があると、磨き残しや削りすぎを生じることとなり出来
上がりの品質に問題が生じる。これに対処するために特
開平４ー２１７４５４号公報（従来技術１）のように、
ロボットの手先に取り付ける工具にばねやダンパーから
成るコンプライアンス機構を持たせ、これにより位置誤
差のあるワークとの接触を保つようにし、ロボットは軌
道制御を行う方法があった。この方法の問題点は、受け
た力に対する工具の速度の応答周波数は、工具のコンプ
ライアンス機構が持つ三つのパラメータである固有周波
数、粘性定数、イナーシャ（以後コンプライアンス特性
と呼ぶ）により固定され、作業に応じて最適なコンプラ
イアンス特性に変更できないことである。

【０００３】従来技術１の課題を解決する方法として、
特開昭６２ー３５９１５号公報（従来技術２）に示され
ているようなロボットの手先に６軸力センサを取り付
け、その先に工具を取り付けて、作業中に工具に加わる
力を力センサで検出し、ソフトウエアによりコンプライ
アンス機構の動きを実現するコンプライアンス制御方法
がある。この方法の場合、受けた力に対する工具の速度
の周波数応答は、ロボットの制御の応答周波数で決まっ
てしまう。応答周波数が低ければ磨き残しや削りすぎを
生じる。これを防止するために作業速度を下げる必要が
あり、作業効率を低下させてしまうという課題がある。

【０００４】従来技術２の課題を解決するために、平成
５年１１月第１１回に日本ロボット学会学術講演会の予
稿集（ｐ２６７ーｐ２７０）へ「インピーダンス制御に
基づく冗長マクロマイクロマニピュレータの運動制御」
（従来技術３）という論文が提案された。マクロ/マイ
クロマニピュレータとは、力の検出器をそなえる微動機
構（マイクロ機構）をロボット（マクロ機構）の手先に
つけた機構である。これらの二つの機構の協調制御によ
り、周波数応答が向上する。この論文で提案された制御
方法は、マクロ機構とマクロ／マイクロ機構の二つのイ
ンピーダンス特性（コンプライアンス特性と同義）を持
たせることによりマクロ／マイクロ機構のインピーダン
ス制御（コンプライアンス制御と同義）を実現するもの
である。

【０００５】

【発明が解決しようとする課題】ところで、上記従来技
術３に記載された制御方法を実際の作業に適用する場
合、どのように二つのインピーダンス特性を設定すれば
良いかという課題がある。

【０００６】しかしながら、上記従来技術の何れにおい
ても、この課題については考慮されていなかった。

【０００７】本発明の目的は、マクロ／マイクロ機構か
ら構成された冗長運動機構において、工具の運動として
一つのコンプライアンス特性モデルを与えることにより
コンプライアンス制御を実現し、広い動作領域で高い応
答周波数特性を要求される作業を可能にした冗長運動機
構の制御方法およびその制御装置を提供することにあ
る。

【０００８】また、本発明の他の目的は、マクロ機構の
先に取付けられ、加工作業に適したコンパクトな２自由
度並進駆動機構およびマクロ機構も含めた冗長運動機構
を提供することにある。

【０００９】

【課題を解決するための手段】上記目的を達成するため
に、本発明は、工具によって作業を行うための冗長の運
動自由度を持つ冗長運動機構を制御する制御方法であっ
て、前記冗長運動機構によって作業を実行するために必
要な工具の運動制御モデル［Ｍ，Ｂ(ｋ)，Ｋ］を設定す
る運動制御モデル設定過程と、前記冗長運動機構に取付
けられたセンサーによって計測される工具の運動状態を
示す情報［（ｆ(ｋ)，ｎ(ｋ)），（ｗ(ｋ)，ｖ(ｋ)），
（φ(ｋ)ｈ(ｋ)，ｅ(ｋ)）］を基に、前記設定された工
具の運動制御モデルに従って、各制御サンプリング時刻
における工具の目標合成運動指令値［ｗｄ(ｋ)，ｖｄ
(ｋ)］を算出する目標合成運動算出過程と、この算出さ
れた工具の目標合成運動指令値を、冗長な運動機構と冗
長でない運動機構の目標運動指令値［（ｗｄ(ｋ)，ｖｄ
(ｋ)−ｕｄ(ｋ)）とｕｄ(ｋ)］に分解する分解過程と、
この分解された各機構の目標運動指令値を元に各機構の
各アクチュエータの指令値を導き、これを元に各アクチ
ュエータを駆動制御する制御過程とを有することを特徴
とする。また、本発明は、工具によって作業を行うため
の冗長の運動自由度を持ち、粗動機構と微動機構とを組
合せて構成される冗長運動機構を制御する制御方法であ
って、前記冗長運動機構によって作業を実行するために
必要な工具の運動制御モデル［Ｍ，Ｂ(ｋ)，Ｋ］を設定
する運動制御モデル設定過程と、前記冗長運動機構に取
付けられたセンサーによって計測される工具の運動状態
を示す情報［（ｆ(ｋ)，ｎ(ｋ)），（ｗ(ｋ)，ｖ
(ｋ)），（φ(ｋ)ｈ(ｋ)，ｅ(ｋ)）］を基に、前記設定
された工具の運動制御モデルに従って、各制御サンプリ
ング時刻における工具の目標合成運動指令値［ｗｄ
(ｋ)，ｖｄ(ｋ)］を算出する目標合成運動算出過程と、
この算出された工具の目標合成運動指令値から冗長自由
度を駆動するための各機構の目標運動指令値［（ｗｄ
(ｋ)，ｖｄ(ｋ)−ｕｄ(ｋ)）とｕｄ(ｋ)］に分解する分
解過程と、この分解された各機構の目標運動指令値を元
に各機構の各アクチュエータの指令値を導き、これを元
に各アクチュエータを駆動制御する制御過程とを有する
ことを特徴とする。

【００１０】また、本発明は、前記冗長運動機構の制御
方法における目標運動算出過程において、前記工具の運
動状態を示す情報として、工具の位置情報と工具に作用
する力の情報とからなることを特徴とする。また、本発
明は、工具によって作業を行うための冗長の運動自由度
を持つ冗長運動機構を制御する制御装置であって、前記
冗長運動機構によって作業を実行するために必要な工具
の運動制御モデルとして慣性特性、粘性特性および剛性
特性からなるコンプライアンス特性モデル［Ｍ，Ｂ
(ｋ)，Ｋ］を設定する運動制御モデル設定部と、前記冗
長運動機構に取付けられたセンサーによって計測される
工具の運動状態として力・偶力、速度および位置・姿勢
の情報［（ｆ(ｋ)，ｎ(ｋ)），（ｗ(ｋ)，ｖ(ｋ)），Ｘ
(ｋ)］と作業を行うための工具のとるべき経路情報［Ｘ
ｄ(ｋ)］と工具が作業対象物へ加えるべき力・偶力［ｆ
ｄ(ｋ)，ｎｄ(ｋ)］とを基に、前記運動制御モデル設定
部で設定された工具の運動制御モデルに従って、各制御
サンプリング時刻における工具の目標合成運動指令値
［ｗｄ(ｋ)，ｖｄ(ｋ)］を算出する目標合成運動算出部
と、該算出部で算出された工具の目標合成運動指令値か
ら冗長自由度を駆動するための各機構の目標運動指令値
に［（ｗｄ(ｋ)，ｖｄ(ｋ)−ｕｄ(ｋ)）とｕｄ(ｋ)］分
解する分解部と、該分解部で分解された各機構の目標運
動指令値を元に各アクチュエータを駆動制御する制御部
とを備えたことを特徴とする。

【００１１】また、本発明は、工具によって作業を行う
ための冗長の運動自由度を持ち、粗動機構と微動機構と
を組合せて構成される冗長運動機構を制御する制御装置
であって、前記冗長運動機構によって作業を実行するた
めに必要な工具の運動制御モデルとして慣性特性、粘性
特性および剛性特性からなるコンプライアンス特性モデ
ル［Ｍ，Ｂ(ｋ)，Ｋ］を設定する運動制御モデル設定部
と、前記冗長運動機構に取付けられたセンサーによって
計測される工具の運動状態として力・偶力、速度および
位置・姿勢の情報［（ｆ(ｋ)，ｎ(ｋ)），（ｗ(ｋ)，ｖ
(ｋ)），（φ(ｋ)ｈ(ｋ)，ｅ(ｋ)）］と作業を行うため
の工具のとるべき経路情報［Ｘｄ(ｋ)］と工具が作業対
象物へ加えるべき力・偶力［ｆｄ(ｋ)，ｎｄ(ｋ)］とを
基に、前記運動制御モデル設定部で設定された工具の運
動制御モデルに従って、各制御サンプリング時刻におけ
る工具の目標合成運動指令値［ｗｄ(ｋ)，ｖｄ(ｋ)］を
算出する目標合成運動算出部と、該算出部で算出された
工具の目標合成運動指令値から冗長自由度を駆動するた
めの各機構の目標運動指令値［（ｗｄ(ｋ)，ｖｄ(ｋ)−
ｕｄ(ｋ)）とｕｄ(ｋ)］に分解する分解部と、該分解部
で分解された各機構の目標運動指令値を元に各アクチュ
エータを駆動制御する制御部とを備えたことを特徴とす
る。また、本発明は、冗長の運動自由度を持ち、粗動機
構と微動機構とを組合せて構成される冗長運動機構にお
ける前記微動機構のアクチュエータを駆動制御する方法
であって、前記微動機構における冗長軸の現在値とその
冗長軸の動作領域の特定の点との間の偏差［Ｐｍ(ｋ)−
Ｐｃ］を算出する偏差算出過程と、冗長軸の制御指令値
［ｖｍｄ(ｋ)］に、前記算出された偏差に応じた値をフ
ィードバックして冗長軸を運動させる冗長軸運動過程と
を有することを特徴とする。

【００１２】また、本発明は、冗長の運動自由度を持
ち、粗動機構と微動機構とを組合せて構成される冗長運
動機構における前記微動機構のアクチュエータを駆動制
御する方法であって、前記微動機構における冗長軸の現
在値［Ｐｍ(ｋ)］をセンサーによって検出する現在値検
出過程と、該検出された冗長軸の現在値とその冗長軸の
動作領域の特定の点［Ｐｃ］との間の偏差［Ｐｍ(ｋ)−
Ｐｃ］を算出する偏差算出過程と、冗長軸の制御指令値
［ｖｍｄ(ｋ)］から、前記算出された偏差に応じた値を
減じることによって新たな冗長軸の制御指令値を得る新
たな制御指令値取得過程と、該得られた新たな冗長軸の
制御指令値をもとに冗長軸を運動させる冗長軸運動過程
とを有することを特徴とする。

【００１３】また、本発明は、工具によって作業を行う
ための冗長の運動自由度を持ち、粗動機構と微動機構と
を組合せて構成される冗長運動機構を制御する制御方法
であって、冗長運動機構に対する工具の目標合成運動指
令値［ｗｄ(ｋ)，ｖｄ(ｋ)］を微動機構の運動と冗長な
運動成分［ｕｄ(ｋ)］と微動機構の運動と冗長でない運
動成分［ｗｄ(ｋ)，ｖｄ(ｋ)−ｕｄ(ｋ)］とに分解する
分解過程と、該分解された微動機構の運動と冗長な運動
成分の運動指令値の内の低周波成分［Ｇ(ｚ)Ｉ₃ｕｄ
(ｋ)］と微動機構の運動と冗長でない運動成分の運動指
令値［ｗｄ(ｋ)，ｖｄ(ｋ)−ｕｄ(ｋ)］とに基づく運動
指令値［Ｖｒｄ(ｋ)］を与えて粗動機構のアクチュエー
タを駆動制御する粗動機構制御過程と、前記分解された
微動機構の運動と冗長な運動成分の運動指令値［ｕｄ
(ｋ)］に対して粗動機構の運動の遅れ［ｖｒ(ｋ)］をフ
ィートバックして微動機構のアクチュエータを駆動制御
する微動機構制御過程とを有することを特徴とする。ま
た、本発明は、工具によって作業を行うための冗長の運
動自由度を持ち、粗動機構と微動機構とを組合せて構成
される冗長運動機構を制御する制御方法であって、冗長
運動機構に対する工具の目標合成運動指令値［ｗｄ
(ｋ)，ｖｄ(ｋ)］を微動機構の運動と冗長な運動成分
［ｕｄ(ｋ)］と微動機構の運動と冗長でない運動成分
［ｗｄ(ｋ)，ｖｄ(ｋ)−ｕｄ(ｋ)］とに分解する分解過
程と、該分解された微動機構の運動と冗長な運動成分の
運動指令値の内の低周波成分［Ｇ(ｚ)Ｉ₃ｕｄ(ｋ)］と
微動機構の運動と冗長でない運動成分の運動指令値［ｗ
ｄ(ｋ)，ｖｄ(ｋ)−ｕｄ(ｋ)］とに基づく運動指令値
［Ｖｒｄ(ｋ)］を与えて粗動機構のアクチュエータを駆
動制御する粗動機構制御過程と、前記分解された微動機
構の運動と冗長な運動成分の運動指令値［ｕｄ(ｋ)］に
対して粗動機構の運動の遅れ［ｖｒ(ｋ)］をフィートバ
ックして冗長軸の制御指令値を得、微動機構における冗
長軸の現在値［Ｐｍ(ｋ)］とその冗長軸の動作領域の特
定の点［Ｐｃ］との間の偏差［Ｐｍ(ｋ)−Ｐｃ］を算出
し、前記得られた冗長軸の制御指令値［ｖｍｄ(ｋ)］
に、前記算出された偏差に応じた値をフィードバックし
て冗長軸を運動させる微動機構のアクチュエータを駆動
制御する微動機構制御過程とを有することを特徴とす
る。

【００１４】また、本発明は、前記冗長運動機構の制御
方法において、前記冗長運動機構によって作業を実行す
るために必要な工具の運動制御モデルとして慣性特性、
粘性特性および剛性特性からなるコンプライアンス特性
モデルを設定する運動制御モデル設定過程と、前記冗長
運動機構に取付けられたセンサーによって計測される工
具の運動状態として力・偶力、速度および位置・姿勢の
情報と作業を行うための工具のとるべき経路情報と工具
が作業対象へ加えるべき力・偶力とを基に、前記設定さ
れた工具の運動制御モデルに従って、各制御サンプリン
グ時刻における工具の目標合成運動指令値を算出する目
標合成運動算出過程とを有することを特徴とする。ま
た、本発明は、冗長の運動自由度を持ち、粗動機構と微
動機構とを組合せて構成される冗長運動機構における前
記微動機構のアクチュエータを駆動制御する制御装置で
あって、前記微動機構における冗長軸の現在値とその冗
長軸の動作領域の特定の点との間の偏差を算出する偏差
算出部と、冗長軸の制御指令値に、前記算出された偏差
に応じた値をフィードバックして冗長軸を運動させる冗
長軸運動部とを備えたことを特徴とする。また、本発明
は、冗長の運動自由度を持ち、粗動機構と微動機構とを
組合せて構成される冗長運動機構における前記微動機構
のアクチュエータを駆動制御する制御装置であって、前
記微動機構における冗長軸の現在値をセンサーによって
検出する現在値検出部と、該検出された冗長軸の現在値
とその冗長軸の動作領域の特定の点との間の偏差を算出
する偏差算出部と、冗長軸の制御指令値から、前記算出
された偏差に応じた値を減じることによって新たな冗長
軸の制御指令値を得る新たな制御指令値取得部と、該得
られた新たな冗長軸の制御指令値をもとに冗長軸を運動
させる冗長軸運動部とを備えたことを特徴とする。

【００１５】また、本発明は、工具によって作業を行う
ための冗長の運動自由度を持ち、粗動機構と微動機構と
を組合せて構成される冗長運動機構を制御する制御装置
であって、冗長運動機構に対する工具の目標合成運動指
令値を微動機構の運動と冗長な運動成分と微動機構の運
動と冗長でない運動成分とに分解する分解部と、該分解
された微動機構の運動と冗長な運動成分の運動指令値の
内の低周波成分と微動機構の運動と冗長でない運動成分
の運動指令値とに基づく運動指令値を与えて粗動機構の
アクチュエータを駆動制御する粗動機構制御部と、前記
分解された微動機構の運動と冗長な運動成分の運動指令
値に対して粗動機構の運動の遅れをフィートバックして
微動機構のアクチュエータを駆動制御する微動機構制御
部とを備えたことを特徴とする。また、本発明は、工具
によって作業を行うための冗長の運動自由度を持ち、粗
動機構と微動機構とを組合せて構成される冗長運動機構
を制御する制御装置であって、冗長運動機構に対する工
具の目標合成運動指令値を微動機構の運動と冗長な運動
成分と微動機構の運動と冗長でない運動成分とに分解す
る分解部と、該分解された微動機構の運動と冗長な運動
成分の運動指令値の内の低周波成分と微動機構の運動と
冗長でない運動成分の運動指令値とを与えて粗動機構の
アクチュエータを駆動制御する粗動機構制御部と、前記
分解された微動機構の運動と冗長な運動成分の運動指令
値に対して粗動機構の運動の遅れをフィートバックして
冗長軸の制御指令値を得、微動機構における冗長軸の現
在値とその冗長軸の動作領域の特定の点との間の偏差を
算出し、前記得られた冗長軸の制御指令値に、前記算出
された偏差に応じた値をフィードバックして冗長軸を運
動させる微動機構のアクチュエータを駆動制御する微動
機構制御部とを備えたことを特徴とする。

【００１６】また、本発明は、前記冗長運動機構の制御
装置において、前記冗長運動機構によって作業を実行す
るために必要な工具の運動制御モデルとして慣性特性、
粘性特性および剛性特性からなるコンプライアンス特性
モデルを設定する運動制御モデル設定部と、前記冗長運
動機構に取付けられたセンサーによって計測される工具
の運動状態として力・偶力、速度および位置・姿勢の情
報と作業を行うための工具のとるべき経路情報と工具が
作業対象へ加えるべき力・偶力を基に、前記設定された
工具の運動制御モデルに従って、各制御サンプリング時
刻における工具の目標合成運動指令値を算出する目標合
成運動算出部とを備えたことを特徴とする。また、本発
明は、ベース部材と工具を取り付けた支持部材との間
を、回転対偶で構成される２対の２節連鎖と、ベース部
材と支持部材の相対回転運動を防止するための１対の２
節連鎖とで連結し、平面６節単ループリンク機構で構成
したことを特徴とする２自由度並進駆動機構である。ま
た、本発明は、前記２自由度並進駆動機構における回転
対偶で構成される２節連鎖において、支持部材に作用す
る力を検出するセンサーを、支持部材と隣接するリンク
に取り付けて構成したことを特徴とする。また、本発明
は、前記２自由度並進駆動機構を、多関節型ロボットの
手先に取付けて構成したことを特徴とする冗長運動機構
である。

【００１７】また、本発明は、前記冗長運動機構におい
て、支持部材に作用する力を検出するセンサーを、ロボ
ットと２自由度並進駆動機構との間に取付けて構成した
ことを特徴とする。また、本発明は、ロボットなど粗動
機構（マクロ機構）の手先に力検出機能を有する微動機
構（マイクロ機構）を装着し、微動機構の手先に工具を
装着して構成されたマクロ／マイクロ機構で、与えられ
たコンプライアンス特性をもつ工具の運動を実現するた
めに、以下の手段でコンプライアンス制御を実現するこ
とを特徴とする。即ち、工具の運動の平衡点である位置
と姿勢の偏差［φ(ｋ)ｈ(ｋ)，ｅ(ｋ)］及び力と偶力
［ｆ(ｋ)，ｎ(ｋ)］を入力として与え、剛性係数、粘性
係数、質量係数（慣性係数）からなるコンプライアンス
特性モデル［Ｋ，Ｂ(ｋ)，Ｍ］を作業に応じて設定し、
これらの値からコンプライアンス制御式が決定する。マ
クロ／マイクロ機構の位置と姿勢、力とモーメントが各
制御サンプリング時刻で帰還されると、コンプライアン
ス制御式からマクロ／マイクロ機構への工具の目標合成
速度指令値［ｗｄ(ｋ)，ｖｄ(ｋ)］が算出される。この
合成速度指令値のうち微動機構と冗長な運動成分［ｕｄ
(ｋ)］について、ローパスフィルターを掛けることによ
り低周波数成分［Ｇ(ｚ)Ｉ₃ｕｄ(ｋ)］を抽出し、冗長
でない成分［ｗｄ(ｋ)，ｖｄ(ｋ)−ｕｄ(ｋ)］と合成し
て、マクロ機構への速度指令値［Ｖｒｄ(ｋ)］とする。
微動機構へは、マイクロ機構の運動と冗長な運動成分の
速度指令値［ｕｄ(ｋ)］からマクロ機構の出力速度［ｖ
ｒ(ｋ)］と動作範囲の特定な点からの位置偏差［Ｐｍ
(ｋ)−Ｐｃ］に定数を掛けた値とを差し引いた値を、指
令値として与える。この操作を並進と回転の合成速度指
令値について行う。

【００１８】以上説明したように、前記構成によれば、
コンプライアンス制御式は、工具の運動としてただ一つ
与えられ、このコンプライアンス制御式を各制御サンプ
リング時刻に解いて、工具の目標速度を決定するので、
工具の目標速度をマクロ／マイクロ機構の合成速度によ
り実現し、一つのコンプライアンス特性モデルに基づく
制御を実現することができ、その結果、広い動作領域で
高い応答周波数特性を要求される作業を実現することが
できる。また、前記構成によれば、工具の目標速度をマ
クロ／マイクロ機構の合成運動で実現するために、微動
機構への速度指令は、マクロ機構の運動の遅れを補う指
令値を与え、微動機構の速度指令から、微動機構の動作
範囲の特定点からの位置偏差に定数を掛けた値を減ずる
ようにしたので、微動機構が可動範囲を超えることを防
いで、広い動作領域で高い応答周波数特性を要求される
作業を実現することができる。

【００１９】

【発明の実施の形態】本発明に係る冗長自由度を持つ運
動機構の制御方法の実施の形態について、図を用いて説
明する。図１には、冗長自由度を持つ運動機構の一実施
の形態であるマクロ／マイクロ機構を示す。ここで、マ
クロ機構（粗動機構）１０は例えば多関節型６自由度ロ
ボットで構成され、マイクロ機構（微動機構）２０はロ
ボットの手先についた例えば２自由度並進の微動機構に
よって構成される。そして、マイクロ機構２０の先端に
例えばグラインダー工具４０が取り付けられる。図１に
示すマクロ／マイクロ機構は、６個のサーボモータ等の
回転型アクチュエータを有するマクロ機構１０と、２個
のサーボモータ等の回転型アクチュエータを有するマイ
クロ機構２０とで構成され、合計８個の回転型アクチュ
エータを有する冗長２自由度を持つ６自由度の運動機構
である。なお、アクチュエータは、全て回転型で構成し
ているが、必ずしも回転型で構成する必要はない。

【００２０】マクロ機構１０は、例えばベース１１に対
して旋回台用回転型アクチュエータ（図示せず）で旋回
される旋回台１２と、該旋回台１２の肩部に対して揺動
自在に支持され、肩部に設けられた上腕用回転型アクチ
ュエータ１４の回転出力を減速機を介してまたは直接固
定連結した上腕１３と、前記旋回台１２の肩部に対して
揺動自在に支持され、旋回台１２の肩部に設けられた前
腕用回転型アクチュエータ１６の回転出力を減速機を介
してまたは直接固定連結したリンク（図示せず）と、前
記上腕１３の揺動端に揺動自在に連結され、上記リンク
の揺動端との間において上腕１３と平行に設けられたリ
ンク１７によって平行四辺形リンク機構を構成して連結
された前腕１５と、該前腕の先端に３自由度を有するよ
うに連結された手首（手先）１８と、前記前腕１５上に
取り付けられ、その回転出力を上記手首１８に連結して
手首１８を３自由度でもって回転させることができるよ
うに構成した３つの回転型アクチュエータ１９とを備え
た多関節型６自由度ロボットで構成される。なお、手首
１８の１自由度を、前腕１５の先端部を前腕の基部に対
して軸心を中心にして回転可能に構成して得ても良い。
この場合、手首１８の２自由度の回転型アクチュエータ
１９を前腕１５の先端部に設けてもよい。そして、マイ
クロ機構２０は、先端にグラインダー工具４０を取り付
けた２自由度並進微動機構で構成され、マクロ機構１０
の手首１８に６軸力センサ４２を介して取り付けられ
る。

【００２１】図２には、２自由度並進微動機構で構成さ
れたマイクロ機構２０を示す。アクチュエータであるサ
ーボモータ２１の出力は減速機（図示せず）を介せてリ
ンク２２に伝達される。リンク２３の一端は、リンク２
２の先端と回り対偶により回転自在に連結され、さらに
リンク２３のもう一端は、センサーレバー２４の一端と
回り対偶により回転自在に連結されている。これらの回
り対偶の軸は平行である。センサーレバー２４のもう一
端は、工具支持部材２５に固定される。センサーレバー
２４には歪みゲージ３４が張り付けられ、該歪みゲージ
３４によってリンク２３から工具支持部材２５に作用す
る力が検出できるように構成している。また、アクチュ
エータであるサーボモータ２６、リンク２７、リンク２
８、およびセンサーレバー２９が、サーボモータ２１、
リンク２２、リンク２３、及びセンサーレバー２４と軸
対称になるように配置され、センサーレバー２９の一端
が同じく支持部材２５に固定されている。なおサーボモ
ータ２１及び２６は機構ベース部材３０に固定されてい
る。この構造は、ベース部材３０と工具支持部材２５と
の間を、リンク２２及び２３からなる２節連鎖とリンク
２７及び２８からなる２節連鎖とで、センサーレバー２
４及び２９を介して連結した構造で、これらベース部材
３０と工具支持部材２５との間は、平面６節単ループ平
面リンク機構を構成している。また工具支持部材２５
は、平面リンク機構の運動面に平行な互いに直交する軸
を持つ進み対偶３２、３３を持つリンク３１を介してベ
ース部材３０に結合されている。即ち、平面６節リンク
機構では、支持部材は平面内の並進と平面に垂直方向の
軸回りの回転運動を行うが、互いに直交する並進待遇１
１及び１２を付加することにより、支持部材５は機構ベ
ース部材１０に対して上下及び水平の２自由度並進運動
のみを行う様になり、支持部材５の機構ベース部材に対
する姿勢は常に一定に保たれる。

【００２２】工具駆動機構の運動を解析するためのモデ
ルを図３に示す。図３に示すＬ１、Ｌ２、Ｌ３、Ｌ４、
Ｌ５、Ｌ６は各リンク２２、２３、２５、２８、２７、
３０の名称とともに、リンクの長さを表すことにする。
図２（ｂ）に示すリンク３１と進み対偶３２、３３の部
分は、変位解析において、工具支持部材２５の姿勢を一
定に保つという条件を与える。図３に示すように、アク
チュエータ２１、２６の各々によって回転駆動される回
転軸角度は、ベース部材Ｌ６（３０）とリンクＬ１（２
２）の成す角θ１と、ベース部材Ｌ６（３０）とリンク
Ｌ５（２７）の成す角θ６とする。また、図３に示すよ
うに、機構ベース部材Ｌ６（３０）上に直交座標系ＸＹ
Ｚを定める。機構ベース部材Ｌ６からみた工具支持部材
Ｌ３（２５）の中心の位置を直交座標系のデータ（ｘ，
ｙ，ｚ）で表すと、θ１，θ６と（ｘ，ｙ，ｚ）との関
係は３角関数の演算により定まり、次に示す（数１）式
の形で表される。

【００２３】

【数１】

【００２４】また、この逆演算は、次に示す（数２）式
の形で表される。

【００２５】

【数２】

【００２６】また、工具支持部材Ｌ３の速度（ｄｘ／ｄ
ｔ，ｄｙ／ｄｔ）とアクチュエータ２１、２６の角速度
（ｄθ1／ｄｔ，ｄθ6／ｄｔ）との関係は、次の（数
３）式より求まる。

【００２７】

【数３】

【００２８】アクチュエータ２１、２６の角速度（ｄθ
1／ｄｔ，ｄθ6／ｄｔ）から工具支持部材２５の速度
（ｄｘ／ｄｔ，ｄｙ／ｄｔ）を求める場合は、（数３）
式の２ｘ２ヤコビ行列の逆行列を求めればよい。工具支
持部材Ｌ３にはグラインダー等の工具４０から作業に応
じた力ベクトルａ＝（ａｘ，ａｙ，ａｚ＝０）が作用
し、またリンクＬ２及びＬ４から力ベクトルｂ１及びｂ
２がそれぞれ作用する。これら３つの力ベクトルの和は
力の釣り合いからゼロとなる。力ベクトルｂ１とｂ２の
スカラ量は、センサーレバー２４及び２９に張り付けた
歪みゲージ３４で検出できる。力ベクトルのスカラ量を
ｋ１、ｋ２とする。力ベクトルｂ１の単位方向ベクトル
はリンクＬ１、Ｌ２の位置より、力ベクトルｂ２の単位
方向ベクトルはＬ５、Ｌ４の位置より求められる。これ
らの単位方向ベクトルを（ｄｘ，ｄｙ，ｄｚ＝０）、
（ｅｘ，ｅｙ，ｅｚ＝０）とすると、グラインダー等の
工具４０に作用する力ベクトルａ＝（ａｘ，ａｙ，ａ
ｚ）は以下の（数４）式のように導かれる。

【００２９】

【数４】

【００３０】図２に示す機構では検出できる力は図３に
示された直交座標系Ｘ、Ｙ、ＺのＸＹ平面内の並進力の
みである。６自由度の力とモーメントのフィードバック
が必要な作業へは、ロボット等で構成されたマクロ機構
１０と微動機構で構成されたマイクロ機構２０との間に
６軸力センサ４２を取り付け、６軸力センサー４２から
検出した６自由度の力とモーメント成分のうち、微動機
構２０の力センサー３４が検出した２自由度力成分と冗
長となる成分は利用せず、６自由度の内、その成分のみ
を微動機構２０の力センサー３４から検出される検出力
を用いる。

【００３１】次に、６自由度の力とモーメント成分が得
られた場合を例として制御方法について図４を用いて説
明する。図４は、図１に示す冗長運動機構であるマクロ
／マイクロ機構を制御する制御装置５０の一実施例を示
すブロック線図である。

【００３２】ここで、工具４０の重心上においた座標系
Σｗを基準として、連続系でインピーダンスモデルを立
てる。工具４０が被加工物（作業対象物）に接触して受
ける力をｆ∈Ｒ³、該力の基準値をｆｄ∈Ｒ³とする。実
際の工具の重心の位置と姿勢を、Ｐ，Ｒとする。そし
て、Ｐｄ，Ｒｄを工具の目標位置、目標姿勢とする。従
って、工具の質量をｍとおくと、工具４０の並進運動の
インピーダンスモデルは、次に示す（数５）式で表され
る。

【００３３】

【数５】

【００３４】なお、Ｋｔは、次に示す（数８）式から与
えられ、Ｃｔは、次に示す（数９）式から与えられる。
また、（ｍω×ｖ）は、回転によって生じる力の非線形
項である。Ｋｔ（Ｐ−Ｐｄ）は、並進運動の剛性成分で
あり、Ｃｔ・ｖは、並進運動の粘性成分である。また、
工具４０が受ける偶力（モーメント）をｎ∈Ｒ³とし、
この偶力の基準値をｎｄ∈Ｒ³とする。重心回りの慣性
テンソルをＩ（（数１０）式）とする。φｈの各成分に
は、バネ定数Ｋｒを乗じたベクトルで表された復元モー
メントがかかり、回転速度に比例してモーメントが減衰
するとして、工具４０の回転運動のインピーダンスモデ
ルは、次に示す（数６）式で表される。

【００３５】

【数６】

【００３６】なお、Ｋｒは、次に示す（数８）式から与
えられ、Ｃｒは、次に示す（数９）式から与えられる。
（ω×Ｉω）は、回転によって生じるモーメントの非線
形項である。Ｋｒφｈは、回転運動の剛性成分であり、
Ｃｒωは、回転運動の粘性成分である。φｈは、工具４
０の目標姿勢Ｒｄから現在姿勢Ｒまでの回転を表し、φ
とｈとは、次に示す（数７）式によって求めることがで
きる。（数７）式により求める演算を、φｈ＝Ｒot（Ｒ
ｄ，Ｒ）とする。

【００３７】

【数７】

【００３８】次に、コンプライアンス制御式演算部５１
において、サンプリング周期をＴとして、マクロ機構１
０およびマイクロ機構２０のアクチュエータへの指令速
度（工具４０の目標とする回転速度および並進速度の３
次元ベクトルｗｄ’（ｋ）、ｖｄ’（ｋ））を算出する
ことについて説明する。ここでは、離散系で考える。

【００３９】ところで、コンプライアンス制御式演算部
５１に対して、グラインダー等の工具４０の重心上に座
標系をおき、この座標系（図１の４４）での工具４０の
運動の仮想のコンプライアンス特性モデルとして、次に
（数８）式に基づく６ｘ６行列Ｋの剛性行列、（数９）
式に基づく６ｘ６行列Ｃの粘性行列、（数１０）式に基
づく３ｘ３行列Ｉの慣性テンソルが与えられる。

【００４０】

【数８】

【００４１】

【数９】

【００４２】

【数１０】

【００４３】なお、Ｋｒは、（数８）式で示すように、
工具４０に対して与えられる３自由度（３軸回り）の回
転（姿勢）に関する各軸の剛性係数（ｋr₁，ｋr₂，ｋ
r₃）のマトリックスで示され、Ｋｔは、（数８）式で示
すように、工具４０に対して与えられる３自由度（３軸
方向）の位置に関する各軸の剛性係数（ｋt₁，ｋt₂，ｋ
t₃）のマトリックスで示される。Ｃｒは、（数９）式で
示すように、工具４０に対して与えられる３自由度（３
軸回り）の回転速度に対して与えられる各軸の粘性係数
（ｃr₁，ｃr₂，ｃr₃）のマトリックスで示され、Ｃｔ
は、（数９）式で示すように、工具４０に対して与えら
れる３自由度（３軸方向）の並進速度に対して与えられ
る粘性係数（ｃt₁，ｃt₂，ｃt₃）のマトリックスで示さ
れる。Ｉは、（数１０）式で示すように、工具４０に対
して与えられる３自由度（３軸回り）の回転の各軸の慣
性係数（ｉ₁，ｉ₂，ｉ₃）のマトリックスで示される。

【００４４】まず、座標変換部６２で工具の座標系Σｗ
に変換されて得られる検出された力と偶力とのそれぞれ
を、ｋ番目の制御サンプリングにおいて、３次元ベクト
ルで（ｆ(ｋ)，ｎ(ｋ)）とする。また、（ｗ(ｋ)，ｖ
(ｋ)）は、座標変換部６４から工具の座標系に変換され
て得られるｋ番目の制御サンプリングにおける工具４０
の回転速度、並進速度を表す３次元ベクトルである。ま
た、Ｘ'(ｋ)は、ｋ番目の制御サンプリングにおける工
具４０の現在の位置、および姿勢を表すマトリックスで
あり、次の（数１１）式で表される。ここで、マイクロ
機構２０の手先の座標系Σｅの現在位置と姿勢をＰｒ
(ｋ)とＲｒ(ｋ)、Σｅで表したマイクロ機構２０の手先
座標系Σｔの現在位置と姿勢をＰｍ(ｋ)とＲｍ(ｋ)とす
る。

【００４５】

【数１１】

【００４６】Ｘ(ｋ)は、Ｘ'(ｋ)を座標変換部６６で工
具の重心座標系Σｗへ変換することにより得られる。ま
た、Ｘｄ(ｋ)は、工具４０によって被加工物（作業対象
物）に対して加工すべきｋ番目の制御サンプリングにお
ける工具４０の目標の位置、および姿勢を表すマトリッ
クスであり、次に示す（数１２）式で表される。即ち、
Ｘｄ(ｋ)は、工具によって加工すべき目標形状を工具の
座標系Σｗによって表したもので、ＣＡＤなどの設計デ
ータから与えられることになる。

【００４７】

【数１２】

【００４８】また、（ｆｄ(ｋ)，ｎｄ(ｋ)）は、工具４
０が作業対象物（被加工物等）に加えるべき力と偶力で
あり、予め、設定される工具の重心座標系Σｗの原点を
基準とする力と偶力の基準値とする。

【００４９】以上説明したことから、コンプライアンス
制御式演算部５１において、サンプリング周期をＴとし
て、工具４０の目標とする回転速度および並進速度の３
次元ベクトル（ｗｄ'(ｋ)，ｖｄ'(ｋ)）は、上記（数
５）式、および（数６）式の関係から、工具４０を移動
させる目標とするｋサンプリング時における指令姿勢／
位置Ｘｄ(ｋ)と、座標変換部６２から工具の重心座標系
Σｗの原点を基準とするｋサンプリング時における工具
４０に作用する力（ｆ(ｋ)，ｎ(ｋ)）、座標変換部６４
から工具座標系で得られるｋサンプリング時における工
具４０のΣｗの現在の回転速度／並進速度（ｗ(ｋ)，ｖ
(ｋ)）、および座標変換部６６から工具の座標系で得ら
れるｋサンプリング時における工具４０のΣｗの現在の
姿勢／位置Ｘ(ｋ)とから、（数８）式、（数９）式、お
よび（数１０）式に基づく仮想のコンプライアンス特性
モデルより次に示す（数１３）式に基づいて算出される
ことになる。

【００５０】

【数１３】

【００５１】Ｍは、質量係数（慣性係数）のマトリック
スを示し、Ｂ(ｋ)は粘性係数のマトリックスを示し、Ｋ
は剛性係数のマトリックスを示す。また、ｅ(ｋ)は、Ｃ
ＡＤなどの設計データから目標指令値として与えられる
工具の重心位置座標で表される工具の目標位置Ｐｄ(ｋ)
と座標変換部６６から得られる工具の現在位置Ｐ(ｋ)と
の間の偏差を示し、φ(ｋ)ｈ(ｋ)は、ＣＡＤなどの設計
データから指令値として与えられる工具の重心位置座標
で表される工具の目標姿勢Ｒｄ(ｋ)と座標変換部６６か
ら得られる工具の現在姿勢Ｒ(ｋ)との間の偏差を示す。
また、（ｆｄ(ｋ)，ｎｄ(ｋ)）は、予め、設定される力
とモーメントの基準値とする。また、Ｔはサンプリング
時間、ｍは工具の仮想の質量である。

【００５２】以上説明したように、コンプライアンス制
御式演算部５１において、算出された工具４０の目標と
する合成運動速度指令値である回転速度および並進速度
の３次元ベクトル（ｗｄ'(ｋ)，ｖｄ'(ｋ)）は、座標変
換部５２によって基準点をマクロ機構手先座標系Σｔの
原点とし、マイクロ機構の基準座標系で表すように変換
されてマクロ機構１０／マイクロ機構４０への回転・並
進運動の合成速度指令値（ｗｄ(ｋ)，ｖｄ(ｋ)）とな
る。ここで並進速度指令値ｖ(ｋ)のうち、並進運動をす
る微動機構４０の運動成分と冗長な（重なる）運動成分
の速度指令値をｕｄ(ｋ)とし、これを演算部５３におい
て次に示す（数１４）に基づいて求める。即ち、速度指
令の冗長成分ｕｄ(ｋ)は、図３に示す直交座標系Ｘ、
Ｙ、Ｚ、すなわちΣｅをマクロ機構（ロボット）１０の
直交座標系Σｂで表した３×３座標変換行列はＲｒ(ｋ)
であるから以下の（数１４）式から導かれる。

【００５３】

【数１４】

【００５４】次に、マクロ機構（ロボット）１０のアク
チュエータの運動によりマクロ機構のリンクの固有振動
が迎起されることを防ぐために、マクロ機構１０へ与え
る速度指令Ｖｒｄ(ｋ)を、ｕｄ(ｋ)を一次遅れ系のロー
パスフィルター５４に通して、低周波数成分を抽出した
もの（Ｇ(ｚ)Ｉ₃ｕｄ(ｋ)）と、減算部６８から得られ
る微動機構と冗長でない運動成分の並進速度指令値（ｖ
ｄ(ｋ)−ｕｄ(ｋ)）とを加算部６９において与える。一
次遅れ系のローパスフィルター５４のパルス伝達関数を
Ｇ（ｚ）とし、マクロ機構（ロボット）１０への指令速
度を６次元ベクトルＶｒｄ(ｋ)とすると、次の（数１
５）式の関係となる。即ち、加算部６９において、微動
機構と冗長でない運動成分の並進速度指令値（ｖｄ(ｋ)
−ｕｄ(ｋ)）と、一次遅れ系のローパスフィルター５４
に通して抽出されたもの（Ｇ(ｚ)Ｉ₃ｕｄ(ｋ)）とを加
算することによって、微動機構２０の並進運動に比較し
てゆっくりした運動をマクロ機構（ロボット）１０にさ
せることができる。これにより、微動機構２０の可動範
囲を拡げることなく、工具４０に対して広い可動範囲で
もって並進運動をさせることが可能となる。

【００５５】

【数１５】

【００５６】そして、加算部６９において得られたマク
ロ機構１０へ与える速度指令（Ｖｒｄ(ｋ)）は、マクロ
機構（ロボット）１０の逆ヤコビ行列Ｊｅ~¹によって角
速度に変換され、マクロ機構１０への角速度指令値とな
る。減算部７０において角速度指令値と演算部５９から
得られる現在の角速度との間の偏差が取られ、演算部５
６において６軸からなる各軸用のアクチュエータに対す
る角速度指令値（比例値Ｐと積分値Ｉとが加算された指
令値）が算出されてｋサンプリング時にホールド回路５
７にホールドされる。そして、マクロ機構（ロボット）
１０の各アクチュエータ１４、１６、１９が、ホールド
回路５７にホールドされた指令値に基づいて駆動されて
工具４０に対して並進／回転運動をさせることになる。
更に、マクロ機構（ロボット）１０の各アクチュエータ
に連結された回転角度検出器から検出された各軸の現在
の角度変位が座標変換部６７および演算部５９に印加さ
れる。演算部５９においては、入力された現在の角度変
位を微分（（(１−ｚ~¹)／Ｔ）・Ｉ₆）することによっ
てマクロ機構１０による工具の現在の角速度が出力され
ることになる。更に、座標変換部６７においては、工具
の座標系に変換されてマクロ機構１０による工具４０の
現在の姿勢／位置（Ｒｒ(ｋ)，Ｐ(ｋ)）が出力されて、
演算部６５に入力されることになる。更に、演算部６０
においては、マクロ機構１０による工具の現在の角速度
からマクロ機構のヤコビ行列Ｊｒに従ってマクロ機構１
０による工具の現在の並進速度ｖｒ(ｋ)が算出されて演
算部７１および６３に入力されることになる。

【００５７】ところで、マクロ機構（ロボット）１０の
力センサ４２によって検出されたマイクロ機構２０のセ
ンサーと冗長成分以外の力およびモーメント（ｆｒ
(ｋ)，ｎｒ(ｋ)）が演算部６１に入力されることにな
る。

【００５８】マイクロ機構（微動機構）２０への指令速
度ｖｍｄ(ｋ)は、演算部７１において、次に示す（数１
６）式に基づいて、演算部５３から得られる冗長速度成
分ｕｄ(ｋ)からマクロ機構（ロボット）１０が出力した
冗長成分ｖｒ(ｋ)を差し引いてマイクロ機構２０の直交
座標へ変換した３次元速度ベクトルとして与えられる。
即ち、マイクロ機構２０への指令速度をｖｍｄ(ｋ)は、
演算部５３から得られる冗長速度成分をｕｄ(ｋ)とし、
マクロ機構（ロボット）の現在の角速度から演算部６０
でロボットのヤコビ行列変換して得られるマイクロ機構
による工具の現在の並進速度をｖｒ(ｋ)とすると、次の
（数１６）式によって導かれる。

【００５９】

【数１６】

【００６０】ここで、定常状態では、マイクロ機構２０
が可動領域の中点Ｐｃ＝（ｐｘ，ｐｙ，０）に戻ってく
るようにするために、マイクロ機構２０の現在位置をＰ
ｍ(ｋ)＝（ｘ(ｋ)，ｙ(ｋ)，０）として、減算部７２に
おいて、指令値ｖｍｄ(ｋ）より、位置の各成分につい
て現在値の中点（Ｐｃ）からの偏差（Ｐｍ（ｋ)−Ｐ
ｃ）にゲインｇを掛けた値（演算部８１において、次に
示す（数１７）式に基づいて算出される。）を減ずる。
即ち、減算部７２において、次に示す（数１８）式の処
理を行ったものを改めて指令速度ｖｍｄ（ｋ）とする。
なお、Ｐｃは、マイクロ機構２０における動作範囲の中
点位置であり、入力設定することは可能である。また、
減算部７２において、指令値ｖｍｄ(ｋ)から、位置の各
成分について現在値の中点（Ｐｃ）からの偏差（Ｐｍ
(ｋ)−Ｐｃ）にゲインｇを掛けた値（演算部８１におい
て、次に示す（数１７）式に基づいて算出される。）を
減ずることによって、定常状態において、マイクロ機構
をその動作範囲の中点へ復帰させるように制御すること
が可能となる。

【００６１】

【数１７】

【００６２】

【数１８】

【００６３】ところで、上記（数１７）式におけるゲイ
ンｇを、ローパスフィルター５４におけるゲインである
パルス伝達関数Ｇ(ｚ)を考慮して最適化する必要があ
る。上記説明では、Ｐｃは、マイクロ機構２０における
動作範囲の中点位置としたが、動作範囲を決めるための
点であれば良い。また、マイクロ機構２０において、ベ
ース部材３０に設けられたセンサー（図示せず）によっ
て工具支持部材２５の偏差（Ｐｍ(ｋ)−Ｐｃ）を直接検
出するように構成することも可能である。このようにマ
イクロ機構２０のとるべき目標指令速度ｖｍｄ(ｋ)が定
まると、演算部７３において、マイクロ機構の逆ヤコビ
行列ｅＪｔ~¹により各アクチュエータの指令角速度が算
出される。そして、減算部７４において、各アクチュエ
ータの指令角速度と演算部７９から得られるマイクロ機
構２０の現在の角速度との偏差が取られ、演算部７５に
おいて２軸からなる各軸用のアクチュエータ２１、２６
に対する角速度指令値（比例値Ｐと積分値Ｉとが加算さ
れた指令値）が算出されてｋサンプリング時にホールド
回路７６にホールドされる。そして、マイクロ機構２０
の各アクチュエータ２１、２６が、ホールド回路７６に
ホールドされた指令値に基づいて駆動されて工具４０に
対して並進微動運動をさせることになる。

【００６４】更に、マイクロ機構２０の各アクチュエー
タ２１、２６に連結された回転角度検出器から検出され
た各軸の現在の角度変位が座標変換部７８および演算部
７９に印加される。演算部７９においては、入力された
現在の角度変位を微分（（(１−ｚ~¹)／Ｔ）・Ｉ₂）す
ることによってマイクロ機構２０の現在の角速度が出力
されることになる。更に、座標変換部７８においては、
上記（数１）式に基づいて、工具座標系に変換されてマ
イクロ機構２０による工具４０の現在の位置（Ｐｍ
(ｋ)）が出力されて、演算部６５および８１に入力され
ることになる。なお、座標変換部７８は、演算部６５に
対しては工具の現在の姿勢および位置（Ｒｍ(ｋ)，Ｐｍ
(ｋ)）が与えられる。更に、演算部８０においては、演
算部７９から得られるマイクロ機構２０の各アクチュエ
ータの現在の角速度からマイクロ機構２０のヤコビ行列
Ｊｍ（上記（数３）式の逆行列）に従って工具の並進速
度ｖｍ(ｋ)が算出されて演算部６３に入力されることに
なる。

【００６５】ところで、マイクロ機構２０の力センサ３
４によって検出された力（ｆｍ(ｋ)）が演算部６１に入
力されることになる。なお、演算部６３は、演算部６０
から得られるマクロ機構１０による工具の現在の回転速
度および並進速度（ｗｒ(ｋ)，ｖｒ(ｋ)）と、演算部８
０から得られるマイクロ機構による工具の現在の並進速
度（ｖｍ(ｋ)）とから、次に示す（数１９）式に基づい
てΣｔの原点を基準としてΣｂで表した工具の現在の回
転速度および並進速度（ｗ(ｋ)，ｖ(ｋ)）が算出され
る。そして、座標変換部６４において、工具の座標系に
おける工具の現在の回転速度および並進速度（ｗ(ｋ)，
ｖ(ｋ)）から、工具の重心座標系Σｗの原点を基準とし
た回転速度および並進速度（ｗ(ｋ)，ｖ(ｋ)）に座標変
換される。

【００６６】

【数１９】

【００６７】また、演算部６１は、マクロ機構１０の力
センサ４２によって検出されたマイクロ機構のセンサー
と冗長成分以外の力および偶力（ｆｒ(ｋ)，ｎｒ(ｋ)）
と、マイクロ機構２０の力センサ３４によって検出され
た力（ｆｍ(ｋ)）とから、次に示す（数２０）式に基づ
いてΣｔの原点を基準としてΣｂで表した工具に作用す
る力および偶力（ｆ(ｋ)，ｎ(ｋ)）が算出される。そし
て、座標変換部６２において、工具の座標系における工
具に作用する力および偶力（ｆ(ｋ)，ｎ(ｋ)）から、工
具の重心座標系Σｗの原点を基準とした工具に作用する
力および偶力（ｆ(ｋ)，ｎ(ｋ)）に座標変換される。

【００６８】

【数２０】

【００６９】また、演算部６５は、座標変換部６７から
得られるマクロ機構による手先の現在の姿勢および位置
（Ｒｒ(ｋ)，Ｐｒ(ｋ)）と、座標変換部７８から得られ
るマイクロ機構による工具の現在の姿勢および位置（Ｒ
ｍ(ｋ)，Ｐｍ(ｋ)）とから、前述の（数１１）式に基づ
いてΣｂにおけるΣｔの座標として工具の現在の姿勢お
よび位置を表す同次座標（Ｘ(ｋ)）が算出される。そし
て、座標変換部６６において、Σｔの現在の姿勢および
位置を表す同次座標（Ｘ'(ｋ)）から、Σｗの現在の姿
勢および位置を表す同次座標（Ｘ(ｋ)）に座標変換され
る。なお、Ｒｍは、マクロ機構１０の手先Σｅから見た
マイクロ機構２０の手先Σｔの姿勢であり、Ｐｍは、マ
クロ機構１０の手先から見たマイクロ機構２０の手先Σ
ｔの原点の位置である。

【００７０】以上説明したように、６自由度の運動を行
うマクロ機構（ロボット）１０の出力である手先に、冗
長自由度（例えば冗長２自由度）の運動を行うマイクロ
機構２０を取り付けて構成した冗長自由度を持つ運動機
構（マクロ／マイクロ機構）において、一つのコンプラ
イアンス特性モデルを設定することによって、マイクロ
機構をマクロ機構に協調制御することが可能となり、そ
の結果、マイクロ機構により応答周波数を向上させたマ
イクロ運動をさせることができる。また、マイクロ機構
による冗長運動について、マクロ機構に対してローパス
フィルタによってなまらせて運動させることによって、
マイクロ機構の動作領域以上の領域において、応答周波
数を向上させたマイクロ運動をさせることが可能とな
る。

【００７１】また、上記実施の形態では、マイクロ機構
２０は、マクロ機構１０の２自由度の並進運動と冗長さ
せた２自由度の並進機構で構成したが、２自由度に限定
されるものでもなく、また並進機構のみに限定されるも
のでない。即ち、マイクロ機構２０は、３自由度の並進
機構で構成してもよく、また２自由度の並進・回転機構
で構成してもよい。しかしながら、マイクロ機構２０に
工具を取り付けて、被加工物に対して加工を施す場合に
は、マイクロ機構２０として２自由度の並進機構で構成
するのが望ましい。また、図４には、マクロ／マイクロ
機構の制御ブロック線図を示すが、コンプライアンス制
御式演算部５１、演算部５３、ローパスフィルタ５４、
演算部５５、５６、５９、６０、６１、６３、６５、７
１、７３、７５、７９、８０、８１、減算部６８、７
０、７２、７４、加算部６９、座標変換部５２、６２、
６４、６６、６７、７８は、計算機において、ソフト処
理することによって実現しても良い。また、部分的に専
用の回路で構成しても良い。また、図４に示すスイッチ
は、離散系を示すものである。

【００７２】

【発明の効果】本発明によれば、マイクロ機構をマクロ
機構の手先に装着して冗長運動機構として構成されるマ
クロ／マイクロ機構において、ユーザが一つのコンプイ
ライアンス特性モデルを与える（設定する）ことによ
り、マクロ機構単体の場合とまったく同じマクロ機構コ
マンドで、コンプライアンス協調制御を可能にし、その
結果、広い動作領域で高い応答周波数特性を要求される
作業を可能にする効果を奏する。また、本発明によれ
ば、定常状態で、マイクロ機構をその動作範囲の特定点
へ復帰するように制御することにより、マイクロ機構が
その動作領域を超えることなく、マクロ機構の動作領域
を高い応答周波数で運動することができる効果を奏す
る。

【００７３】また、本発明によれば、マイクロ機構を２
自由度並進駆動機構で構成することによって、工具を高
い応答周波数で運動させることができ、その結果、加工
作業に適するコンパクトなマイクロ機構を実現すること
ができる。また、本発明によれば、２自由度並進駆動機
構で構成されたマイクロ機構を多関節型ロボットの手先
に装着することによって、広い動作領域で高い応答周波
数特性を要求される加工作業に適するマクロ／マイクロ
機構を実現することができる。

【図面の簡単な説明】

【図１】本発明に係るマクロ／マイクロ機構の一実施例
の概略構成を示した斜視図である。

【図２】本発明に係るマイクロ機構の一実施例である２
自由度微動機構を示す図である。

【図３】図２に示す２自由度微動機構の解析モデルを示
す図である。

【図４】本発明に係るマクロ／マイクロ機構の制御装置
の一実施例を示すブロック図である。

【符号の説明】

１０…マクロ機構（ロボット）、１１…ベース、１２…
旋回台、１３…上腕、１４…上腕用回転型アクチュエー
タ、１５…前腕、１６…前腕用回転型アクチュエータ、
１８…手首、１９…回転型アクチュエータ、２０…マイ
クロ機構（微動機構）、２１…サーボモータ、２２、２
３…リンク、２４…センサーレバー、２５…工具支持部
材、２６…サーボモータ、２７、２８…リンク、２９…
センサーレバー、３０…ベース部材、３１…リンク、３
２、３３…進み対偶、３４…歪みゲージ（力センサ）、
４０…工具、４２…６軸力センサ、５０…制御装置、５
１…コンプライアンス制御式演算部、５２、６２、６
４、６６、６７、７８…座標変換部、５３、５５、５
６、５９、６０、６１、６３、６５、７１、７３、７
５、７９、８０、８１…演算部、５４…ローパスフィル
ター、５７、７６…ホールド回路、６８、７０、７２、
７４…減算部、６９…加算部。

Claims (19)

【特許請求の範囲】
1. 【請求項１】工具によって作業を行うための冗長の運動
   自由度を持つ冗長運動機構を制御する制御方法であっ
   て、 前記冗長運動機構によって作業を実行するために必要な
   工具の運動制御モデルを設定する運動制御モデル設定過
   程と、 前記冗長運動機構に取付けられたセンサーによって計測
   される工具の運動状態を示す情報を基に、前記設定され
   た工具の運動制御モデルに従って、各制御サンプリング
   時刻における工具の目標運動指令値を算出する目標運動
   算出過程と、 この算出された工具の目標運動指令値を冗長な運動機構
   と冗長でない運動機構の目標運動指令値に分解する分解
   過程と、 この分解された各機構の目標運動指令値を元に各機構の
   各アクチュエータを駆動制御する制御過程とを有するこ
   とを特徴とする冗長運動機構の制御方法。
2. 【請求項２】工具によって作業を行うための冗長の運動
   自由度を持ち、粗動機構と微動機構とを組合せて構成さ
   れる冗長運動機構を制御する制御方法であって、 前記冗長運動機構によって作業を実行するために必要な
   工具の運動制御モデルを設定する運動制御モデル設定過
   程と、 前記冗長運動機構に取付けられたセンサーによって計測
   される工具の運動状態を示す情報を基に、前記設定され
   た工具の運動制御モデルに従って、各制御サンプリング
   時刻における工具の目標運動指令値を算出する目標運動
   算出過程と、 この算出された工具の目標運動指令値から冗長自由度を
   駆動するための粗動機構と微動機構の目標運動指令値に
   分解する分解過程と、 この分解された各機構の目標運動指令値を元に各機構の
   各アクチュエータを駆動制御する制御過程とを有するこ
   とを特徴とする冗長運動機構の制御方法。
3. 【請求項３】請求項１または２記載の目標運動算出過程
   において、前記工具の運動状態を示す情報として、工具
   の位置、速度および工具に作用する力の情報からなるこ
   とを特徴とする冗長運動機構の制御方法。
4. 【請求項４】工具によって作業を行うための冗長の運動
   自由度を持つ冗長運動機構を制御する制御装置であっ
   て、 前記冗長運動機構によって作業を実行するために必要な
   工具の運動制御モデルとして剛性特性、粘性特性および
   慣性特性からなるコンプライアンス特性モデルを設定す
   る運動制御モデル設定部と、 前記冗長運動機構に取付けられたセンサーによって計測
   される工具の運動状態として位置、速度および力の情報
   と作業を行うための工具のとるべき経路情報と工具が作
   業対象物に加えるべき力の情報とを基に、前記運動制御
   モデル設定部で設定された工具の運動制御モデルに従っ
   て、各制御サンプリング時刻における工具の目標運動指
   令値を算出する目標運動算出部と、 該算出部で算出された工具の目標運動指令値から冗長自
   由度を駆動するための冗長な運動機構と冗長でない運動
   機構の目標運動指令値に分解する分解部と、 該分解部で分解された各機構の目標運動指令値を元に各
   機構の各アクチュエータを駆動制御する制御部とを備え
   たことを特徴とする冗長運動機構の制御装置。
5. 【請求項５】工具によって作業を行うための冗長の運動
   自由度を持ち、粗動機構と微動機構とを組合せて構成さ
   れる冗長運動機構を制御する制御装置であって、 前記冗長運動機構によって作業を実行するために必要な
   工具の運動制御モデルとして剛性特性、粘性特性および
   慣性特性からなるコンプライアンス特性モデルを設定す
   る運動制御モデル設定部と、 前記冗長運動機構に取付けられたセンサーによって計測
   される工具の運動状態として位置、速度および力の情報
   と作業を行うための工具のとるべき経路情報と工具が作
   業対象物に加えるべき力の情報とを基に、前記運動制御
   モデル設定部で設定された工具の運動制御モデルに従っ
   て、各制御サンプリング時刻における工具の目標運動指
   令値を算出する目標運動算出部と、 該算出部で算出された工具の目標運動指令値から冗長自
   由度を駆動するための粗動機構と微動機構の目標運動指
   令値に分解する分解部と、 該分解部で分解された各機構の目標運動指令値を元に各
   機構の各アクチュエータを駆動制御する制御部とを備え
   たことを特徴とする冗長運動機構の制御装置。
6. 【請求項６】冗長の運動自由度を持ち、粗動機構と微動
   機構とを組合せて構成される冗長運動機構における前記
   微動機構のアクチュエータを駆動制御する方法であっ
   て、 前記微動機構における冗長軸の現在値とその冗長軸の動
   作領域の特定の点との間の偏差を算出する偏差算出過程
   と、 冗長軸の制御指令値に、前記算出された偏差に応じた値
   をフィードバックして冗長軸を運動させる冗長軸運動過
   程とを有することを特徴とする冗長運動機構の制御方
   法。
7. 【請求項７】冗長の運動自由度を持ち、粗動機構と微動
   機構とを組合せて構成される冗長運動機構における前記
   微動機構のアクチュエータを駆動制御する方法であっ
   て、 前記微動機構における冗長軸の現在値をセンサーによっ
   て検出する現在値検出過程と、 該検出された冗長軸の現在値とその冗長軸の動作領域の
   特定の点との間の偏差を算出する偏差算出過程と、 冗長軸の制御指令値から、前記算出された偏差に応じた
   値を減じることによって新たな冗長軸の制御指令値を得
   る新たな制御指令値取得過程と、 該得られた新たな冗長軸の制御指令値をもとに冗長軸を
   運動させる冗長軸運動過程とを有することを特徴とする
   冗長運動機構の制御方法。
8. 【請求項８】工具によって作業を行うための冗長の運動
   自由度を持ち、粗動機構と微動機構とを組合せて構成さ
   れる冗長運動機構を制御する制御方法であって、 冗長運動機構に対する工具の目標運動指令値を微動機構
   の運動と冗長な運動成分と微動機構の運動と冗長でない
   運動成分とに分解する分解過程と、 該分解された冗長な運動成分の運動指令値の内の低周波
   成分と冗長でない運動成分の運動指令値とに基づく運動
   指令値を与えて粗動機構のアクチュエータを駆動制御す
   る粗動機構制御過程と、 前記分解された冗長な運動成分の運動指令値に対して粗
   動機構の運動の遅れをフィートバックして微動機構のア
   クチュエータを駆動制御する微動機構制御過程とを有す
   ることを特徴とする冗長運動機構の制御方法。
9. 【請求項９】工具によって作業を行うための冗長の運動
   自由度を持ち、粗動機構と微動機構とを組合せて構成さ
   れる冗長運動機構を制御する制御方法であって、 冗長運動機構に対する工具の目標運動指令値を微動機構
   の運動と冗長な運動成分と微動機構の運動と冗長でない
   運動成分とに分解する分解過程と、 該分解された冗長な運動成分の運動指令値の内の低周波
   成分と冗長でない運動成分の運動指令値とに基づく運動
   指令値を与えて粗動機構のアクチュエータを駆動制御す
   る粗動機構制御過程と、 前記分解された冗長な運動成分の運動指令値に対して粗
   動機構の運動の遅れをフィートバックして冗長軸の制御
   指令値を得、微動機構における冗長軸の現在値とその冗
   長軸の動作領域の特定の点との間の偏差を算出し、前記
   得られた冗長軸の制御指令値に、前記算出された偏差に
   応じた値をフィードバックして冗長軸を運動させる微動
   機構のアクチュエータを駆動制御する微動機構制御過程
   とを有することを特徴とする冗長運動機構の制御方法。
10. 【請求項１０】請求項８または９記載の冗長運動機構の
    制御方法において、 前記冗長運動機構によって作業を実行するために必要な
    工具の運動制御モデルとして慣性特性、粘性特性および
    剛性特性からなるコンプライアンス特性モデルを設定す
    る運動制御モデル設定過程と、 前記冗長運動機構に取付けられたセンサーによって計測
    される工具の運動状態として位置、速度および力の情報
    と作業を行うための工具のとるべき経路情報と工具が作
    業対象物に加えるべき力の情報とを基に、前記設定され
    た工具の運動制御モデルに従って、各制御サンプリング
    時刻における工具の目標運動指令値を算出する目標運動
    算出過程とを有することを特徴とする冗長運動機構の制
    御方法。
11. 【請求項１１】冗長の運動自由度を持ち、粗動機構と微
    動機構とを組合せて構成される冗長運動機構における前
    記微動機構のアクチュエータを駆動制御する制御装置で
    あって、 前記微動機構における冗長軸の現在値とその冗長軸の動
    作領域の特定の点との間の偏差を算出する偏差算出部
    と、 冗長軸の制御指令値に、前記算出された偏差に応じた値
    をフィードバックして冗長軸を運動させる冗長軸運動部
    とを備えたことを特徴とする冗長運動機構の制御装置。
12. 【請求項１２】冗長の運動自由度を持ち、粗動機構と微
    動機構とを組合せて構成される冗長運動機構における前
    記微動機構のアクチュエータを駆動制御する制御装置で
    あって、 前記微動機構における冗長軸の現在値をセンサーによっ
    て検出する現在値検出部と、 該検出された冗長軸の現在値とその冗長軸の動作領域の
    特定の点との間の偏差を算出する偏差算出部と、 冗長軸の制御指令値から、前記算出された偏差に応じた
    値を減じることによって新たな冗長軸の制御指令値を得
    る新たな制御指令値取得部と、 該得られた新たな冗長軸の制御指令値をもとに冗長軸を
    運動させる冗長軸運動部とを備えたことを特徴とする冗
    長運動機構の制御装置。
13. 【請求項１３】工具によって作業を行うための冗長の運
    動自由度を持ち、粗動機構と微動機構とを組合せて構成
    される冗長運動機構を制御する制御装置であって、 冗長運動機構に対する工具の目標運動指令値を微動機構
    の運動と冗長な運動成分と微動機構の運動と冗長でない
    運動成分とに分解する分解部と、 該分解された冗長な運動成分の運動指令値の内の低周波
    成分と冗長でない運動成分の運動指令値とに基づく運動
    指令値を与えて粗動機構のアクチュエータを駆動制御す
    る粗動機構制御部と、 前記分解された冗長な運動成分の運動指令値に対して粗
    動機構の運動の遅れをフィートバックして微動機構のア
    クチュエータを駆動制御する微動機構制御部とを備えた
    ことを特徴とする冗長運動機構の制御装置。
14. 【請求項１４】工具によって作業を行うための冗長の運
    動自由度を持ち、粗動機構と微動機構とを組合せて構成
    される冗長運動機構を制御する制御装置であって、 冗長運動機構に対する工具の目標運動指令値を微動機構
    の運動と冗長な運動成分と微動機構の運動と冗長でない
    運動成分とに分解する分解部と、 該分解された冗長な運動成分の運動指令値の内の低周波
    成分と冗長でない運動成分の運動指令値とに基づく運動
    指令値を与えて粗動機構のアクチュエータを駆動制御す
    る粗動機構制御部と、 前記分解された冗長な運動成分の運動指令値に対して粗
    動機構の運動の遅れをフィートバックして冗長軸の制御
    指令値を得、微動機構における冗長軸の現在値とその冗
    長軸の動作領域の特定の点との間の偏差を算出し、前記
    得られた冗長軸の制御指令値に、前記算出された偏差に
    応じた値をフィードバックして冗長軸を運動させる微動
    機構のアクチュエータを駆動制御する微動機構制御部と
    を備えたことを特徴とする冗長運動機構の制御装置。
15. 【請求項１５】請求項１３または１４記載の冗長運動機
    構の制御装置において、 前記冗長運動機構によって作業を実行するために必要な
    工具の運動制御モデルとして慣性特性、粘性特性および
    剛性特性からなるコンプライアンス特性モデルを設定す
    る運動制御モデル設定部と、 前記冗長運動機構に取付けられたセンサーによって計測
    される工具の運動状態として位置、速度および力の情報
    と作業を行うための工具のとるべき経路情報と工具が作
    業対象物に加えるべき力の情報とを基に、前記設定され
    た工具の運動制御モデルに従って、各制御サンプリング
    時刻における工具の目標運動指令値を算出する目標運動
    算出部とを備えたことを特徴とする冗長運動機構の制御
    装置。
16. 【請求項１６】ベース部材と工具を取り付けた支持部材
    との間を、回転対偶で構成される２対の２節連鎖と、ベ
    ース部材と支持部材の相対回転運動を防止するための１
    対の２節連鎖とで連結し、平面６節単ループリンク機構
    で構成したことを特徴とする２自由度並進駆動機構。
17. 【請求項１７】請求項１６記載の回転対偶で構成される
    ２節連鎖において、支持部材に作用する力を検出するセ
    ンサーを、支持部材と隣接するリンクに取り付けて構成
    したことを特徴とする２自由度並進駆動機構。
18. 【請求項１８】請求項１６または１７記載の２自由度並
    進駆動機構を、多関節型ロボットの手先に取付けて構成
    したことを特徴とする冗長運動機構。
19. 【請求項１９】請求項１８記載の冗長運動機構におい
    て、支持部材に作用する力を検出するセンサーを、ロボ
    ットと２自由度並進駆動機構との間に取付けて構成した
    ことを特徴とする冗長運動機構。