JPH08118275A

JPH08118275A - マニピュレータの制御装置

Info

Publication number: JPH08118275A
Application number: JP25400694A
Authority: JP
Inventors: Mitsuo Koide; 光男小出; Yuji Tsusaka; 祐司津坂; Hideki Nomura; 秀樹野村; Shoji Asai; 彰司浅井; Norihiko Akao; 憲彦赤尾; Yoshimasa Sato; 義正佐藤
Original assignee: Toyota Motor Corp; Toyota Central R&D Labs Inc
Current assignee: Toyota Motor Corp; Toyota Central R&D Labs Inc
Priority date: 1994-10-19
Filing date: 1994-10-19
Publication date: 1996-05-14
Anticipated expiration: 2017-09-17
Also published as: JP3324298B2

Abstract

(57)【要約】【目的】計算トルク法とトルクセンサフィードバックと
を行う際に、トルクセンサフィードバックを行うだけで
なく、関節アクチュエータの速度フィードバックも同時
に行うことで安定に、かつ高精度なトルク制御を行う。【構成】アーム補償トルク演算手段１０は、一般に計算
トルク法と呼ばれる、ニュートン・オイラー法を用いて
目標とする運動を達成するのに必要なトルク指令値を演
算する。関節トルクフィードバック制御手段９は、目標
トルク生成手段１２より与えられる各軸の関節トルク指
令値と、トルクセンサ３から得られる現在の関節トルク
値とを比較し、関節トルク指令値と現在の関節トルク値
とを一致させるように、モータ電流生成手段８及びモー
タドライバー６を介してモータ１を駆動する。また、ト
ルクセンサフィートバック制御の他にモータの角速度の
フィードバック制御を同時に行うことで安定化を図って
いる。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、マニピュレータの制御
装置に係り、より詳しくは、計算トルク法とトルクセン
サフィードバック制御とを用いて安定かつ高精度にトル
ク制御を行うマニピュレータの制御装置に関する。

【０００２】

【従来の技術】一般に、組み付け用のロボットマニピュ
レータでは、接触作業を行うことが殆どであるが、ワー
クの位置ずれ等のために、目標位置より手前で接触が開
始される場合がある。この時、マニピュレータの位置制
御系が高いサーボ剛性を維持していると、目標位置との
間に生じた位置偏差に応じて、位置制御系は過大な力を
発生し、ワークやマニピュレータを破損することがあっ
た。

【０００３】このため、従来では、ワークを把握するツ
ール部に、コンプライアンス機構を設ける等の工夫が必
要があった。このためワーク毎にツールを開発する必要
があり、設計、製作に多大の時間を要していた。また、
１台のマニピュレータが持てるツールの数には限りがあ
り、このため行える作業数が限定される、という問題が
あった。

【０００４】これらの問題点を解決するためには、マニ
ピュレータ自体に任意のコンプライアンスを設定できる
ようにすればよい。すなわち、接触時には、柔らかいコ
ンプライアンス、つまり低いサーボ剛性で接触するよう
にすればよい。このようにすれば、接触時に、位置偏差
が生じても過大な力を発生することはなく、位置ずれが
あっても、ワークに倣って作業を行うことが可能にな
り、ワークやマニピュレータの破損が避けられるだけで
なく、作業の成功率も向上する。

【０００５】しかしながら、従来の位置制御マニピュレ
ータにおいては、位置サーボ剛性を低下させると位置精
度が低下し、接触前の自由空間において必要な位置精度
が得られない、という問題点があり実用的ではなかっ
た。

【０００６】低いサーボ剛性の場合に位置精度が低下す
る原因は、伝達系の摩擦、マニピュレータに加わる重
力、マニピュレータが運動する時に働く慣性力、遠心
力、コリオリ力等の外乱が作用するためである。これら
の外乱を打ち消すためには、従来の位置制御マニピュレ
ータでは、高いゲインの位置フィードバックを行う必要
があり、結果として高いサーボ剛性を用いざるを得なか
った。

【０００７】これらの外乱のうち伝達系の摩擦以外の部
分、すなわち、重力、慣性力、遠心力、コリオリ力を高
い位置フィードバックゲインを用いずに補償する方法と
しては、計算トルク法を用いて運動時にそれらを逐次計
算し、補償する方法がある。

【０００８】また、伝達系の摩擦を補償する方法として
は、マニピュレータ各軸の減速機の後でトルク検出を行
い、トルクフィードバックを行う方法がある。

【０００９】特開平３−６２２０５では、上記の計算ト
ルク法を用いると共に関節トルクフィードバック制御を
行うことで、マニピュレータに望みの運動をさせること
を提案している。

【００１０】

【発明が解決しようとしている課題】しかしながら、従
来の上記方法では、遅れの大きいトルクセンサ信号のみ
を用いてトルクフィードバックを行っているので、トル
ク制御系の安定化が困難である、という問題点があっ
た。

【００１１】本発明は、従来技術における以上のような
問題点を解決するマニピュレータの制御装置を提供しよ
うとするもので、計算トルク法とトルクセンサフィード
バックとを行う際に、トルクセンサフィードバックを行
うだけでなく、関節アクチュエータの速度フィードバッ
クも同時に行うことで安定に、かつ高精度なトルク制御
を行うものである。

【００１２】

【課題を解決するための手段】上記目的を達成するため
に本発明は、伝達機構を介してリンクを駆動するモータ
と、モータの角度を検出する角度検出手段と、伝達機構
の後の出力トルクを検出するトルク検出手段と、目標と
する運動の加速度指令値とモータの現在角度とマニュピ
レータの慣性パラメータとに基づいて定まる慣性分、及
びモータの現在角度とモータの現在角速度とマニュピレ
ータの慣性パラメータとに基づいて定まる非線形補償分
を演算すると共に、演算された慣性分と非線形補償分と
を加算してトルク指令値を演算するトルク指令値演算手
段と、演算されたトルク指令値とトルク検出手段で検出
された現在の出力トルク値との偏差に基づいてモータの
トルク制御値を演算するトルク制御値演算手段と、演算
されたトルク制御値からモータの現在角速度に基づく値
を減算した指令値をモータに供給する供給手段と、を含
んで構成したものである。

【００１３】

【作用】本発明によれば、トルク検出手段で、伝達機構
の後の出力トルクを検出しているので、伝達機構の摩擦
分も補償することが可能である。

【００１４】トルク指令値演算手段は、目標とする運動
の加速度指令値とモータの現在角度とマニュピレータの
慣性パラメータとに基づいて定まる慣性分、及びモータ
の現在角度とモータの現在角速度とマニュピレータの慣
性パラメータとに基づいて定まる非線形補償分を演算す
ると共に、演算された慣性分と非線形補償分とを加算し
てトルク指令値を演算する。すなわち、トルク指令値演
算手段によって計算トルク法によりマニピュレータが望
ましい運動を行うのに必要な各軸のトルク指令値が厳密
に計算されている。

【００１５】トルク制御値演算手段は、演算されたトル
ク指令値とトルク検出手段で検出された現在の出力トル
ク値との偏差に基づいてモータのトルク制御値を演算
し、供給手段は、演算されたトルク制御値からモータの
現在角速度に基づく値を減算した指令値をモータに供給
する。モータに供給する指令値は、各軸のトルクフィー
ドバック制御により実現されるが、このトルクフィード
バック制御はトルク検出手段で検出された出力トルクだ
けでなく、モータ角速度も同時にフィードバック制御す
ることで制御しているため、遅れの小さいダンピングを
作用させることができ、トルク制御を安定に行うことが
できる。

【００１６】そして、トルク制御が安定化すれば、従来
に比べて、トルクループゲインを十分に上げることがで
き、これにより指令トルクをより精度良く一致させ、伝
達手段の摩擦を十分に補償することができる。

【００１７】従って、位置フィードバックゲインが低く
ても、より高い軌跡精度が得られる。また、運動に必要
なトルクが厳密に実現されるので、高速に動作させても
より高い軌跡精度が得られる。従って、位置サーボ剛性
を低くして高速動作させても軌跡精度が向上する。

【００１８】また、接触作業時において低い位置サーボ
剛性のままでワークに接近できるので、接触後、位置偏
差が生じても、過大なモータ操作力が生じない。従っ
て、ワークやマニピュレータを破損することもなく、ワ
ークに倣って作業を行うことができ、作業の品質、成功
率が向上する。

【００１９】

【実施例】以下図面を参照して本発明の実施例を詳細に
説明する。

【００２０】まず、多軸マニピュレータの少なくとも一
つの軸を制御する場合に本発明を適用した第１実施例の
マニピュレータの制御装置について説明する。

【００２１】マニピュレータ各軸には、リンクとしての
アームを駆動するためのモータが設けられている。図１
に示すように、モータ１の出力軸には、モータの回転角
度を検出する角度検出手段としてのエンコーダ５が取り
付けられている。エンコーダ５から出力された角度信号
は、微分演算器７により角速度信号に変換される。

【００２２】各軸に設けられたモータ１の駆動力は、伝
達機構としての減速機２、減速機２の後で出力トルク値
を検出する関節トルクセンサ３を介して、各軸アーム４
へ伝達され、これにより各アーム４が軸を中心に駆動さ
れる。

【００２３】関節トルクセンサ３は、図３（１）〜
（３）に示すように、中心部から放射状に延びる複数の
梁と直線上に位置する一対の梁以外の梁を連結するリン
グ状の枠とによって構成された梁構造体３１と、梁構造
体３１に取り付けられ直線上に位置する一対の梁との間
のギャップを検出する一対のギャップセンサ３２とから
構成されている。この関節トルクセンサ３は、減速機２
とアーム４との間に、アームを駆動するトルクに比例し
て枠に連結された梁がたわむように梁構造体３１の中心
部を減速機の出力軸に取付け、この梁のたわみをギャッ
プセンサ３２で検出することで関節トルクを検出する。

【００２４】一般に、減速機にはかなり大きな摩擦分が
存在するが、関節トルクセンサ３は減速機２の後、すな
わち出力側に取り付けられているので、減速機の摩擦分
も補償することが可能である。

【００２５】アームが目標とする運動を達成するのに必
要な関節トルク指令値は、トルク指令値演算手段として
のアーム補償トルク演算手段１０により計算される。こ
の計算は、一般に計算トルク法と呼ばれる、例えば、ニ
ュートン・オイラー法を用いて行うことができる。具体
的には、目標とする運動に必要な目標加速度指令値とモ
ータの現在角度とマニュピレータの慣性パラメータとに
基づいて定まる慣性分、及びモータの現在角度とモータ
の現在速度とマニュピレータの慣性パラメータとに基づ
いて定まる非線形補償分（重力分、コリオリ力分、及び
遠心力分）を演算すると共に、演算された慣性分と非線
形補償分とを加算して関節トルク指令値を演算する。そ
して、アーム補償トルク演算手段１０は、演算した関節
トルク指令値を、関節トルクフィードバック制御手段９
が用いるトルク指令値の一部とするために、目標トルク
生成手段１２に入力する。

【００２６】上記目標とする運動に必要な目標加速度指
令値は、マニピュレータアームの望ましい目標軌道を生
成する目標軌道生成手段１８から発生される目標軌道
を、微分演算器１６および微分演算器１７を介して時間
に関して２階微分することで得られる。

【００２７】また、上記のマニピュレータの慣性パラメ
ータは、マニピュレータの幾何学的諸元、質量分布、重
心位置等からアームの現在角度毎に計算され、予めアー
ム補償トルク演算手段１０に設けられているメモリに記
憶されている。

【００２８】関節トルクフィードバック制御手段９で
は、目標トルク生成手段１２より与えられる各軸の関節
トルク指令値と、関節トルクセンサ３から得られる現在
の関節トルク値とを比較し、関節トルク指令値と現在の
関節トルク値とを一致させるように、モータ電流生成手
段８及びモータドライバー６を介してモータ１を駆動す
る。

【００２９】関節トルクフィードバック制御手段９で
は、比例積分微分（ＰＩＤ）制御法等を用いて制御する
ことも可能であるが、従来のようにトルクセンサ出力だ
けを用いてＰＩＤ制御（トルクセンサフィートバック制
御）を行った場合、アクチュエータであるモータとセン
サとの間にはかなりの遅れがあるため、ゲインを上げ過
ぎると不安定化する。そこで、本実施例では、以下に示
すようにトルクセンサフィートバック制御の他にモータ
の角速度のフィードバック制御を同時に行うことで安定
化を図っている。

【００３０】図２にトルクセンサフィートバック制御と
モータの角速度のフィードバック制御とを同時に行なう
関節トルクフィードバック制御手段９の詳細なブロック
図を示す。関節トルクフィードバック制御手段９は、関
節トルク指令値と関節トルクセンサ３で検出された現在
関節トルク値との偏差を演算する減算器９Ａを備えてい
る。減算器９Ａで演算された偏差は、乗算器９Ｂ、９
Ｃ、９Ｄで、各々ＰＩＤフィードバック制御の微分ゲイ
ンＫｄとラプラス演算子ｓとの積ｓＫｄ、比例ゲインＫ
ｐ、積分ゲインＫｉとラプラス演算子ｓとの商Ｋｉ／ｓ
が乗算される。そして、乗算器９Ｂ、９Ｃ、９Ｄの出力
が加算器９Ｅで加算され、加算器９Ｆで加算器９Ｅ出力
と関節トルク指令値とが加算される。

【００３１】本実施例では、関節トルクセンサのＰＩＤ
フィードバック制御の他に、さらに、微分演算器７より
得られるモータの現在角速度に乗算器９Ｇで予め定めら
れたゲインＫｆｖを乗算してネガティブフィードバック
制御を行うループが付け加えられており、減算器９Ｈに
より加算器９Ｆ出力から乗算器９Ｇ出力を減算した値が
アーム駆動分モータトルク指定値として出力される。

【００３２】このネガティブフィードバック制御を行う
ことにより、トルク制御系に速度に比例するダンピング
を作用させることができ、これにより安定化することが
できる。特に、本実施例では、モータ角速度を用いてい
るので、遅れのないフィードバック制御を行なうことが
可能であり、ゲインも上げることができ、十分に安定化
できるようになる。

【００３３】また、減速機の後ろに配置した関節トルク
センサ信号には、伝達系の遅れが存在するが、本実施例
では同時に角速度のフィードバック制御も行なっている
ので、従来のように関節トルクセンサ信号だけを用いて
フィードバック制御する場合に比較してはるかに安定化
できるようになる。従って、トルクループの比例ゲイン
Ｋｐをより大きくできるので、トルク制御がより高精度
にできるようになり、計算トルク法を用いる大きなメリ
ットがある。

【００３４】一方、モータ分の補償トルクは、アーム補
償トルクとは別にモータ補償トルク演算手段１１により
演算される。モータ補償トルク演算手段１１では、目標
加速度とモータのロータイナーシャ分（モータの慣性
分）とを乗算し、モータ慣性を駆動するのに必要なトル
ク分を計算し、このトルク分をモータ電流生成手段８に
入力する。一般に、モータのロータイナーシャは、マニ
ピュレータアームの姿勢によらず一定であるので、簡単
な計算により、モータの慣性分だけを正確に補償するこ
とができる。なお、モータのロータイナーシャは、あら
かじめカタログ値、設計諸元等より与えられる。

【００３５】モータ電流生成手段８では、関節トルクフ
ィードバック制御手段９より入力されるアーム駆動分ト
ルク指令とモータ補償トルク演算手段１１より入力され
るモータ駆動分トルク指令とを加算し、最終的なモータ
電流指令値を演算しモータドライバー６に入力する。

【００３６】モータドライバー６は、モータ電流生成手
段８より入力されたモータ電流指令値とモータに流れる
電流値とが一致するように、モータ１に流れる電流を制
御する。

【００３７】可変位置フィードバック制御手段１３は、
目標軌道生成手段１８の出力である目標軌道とエンコー
ダ５で検出されたモータの現在角度との偏差を位置偏差
として演算し、その位置偏差に適当な位置偏差ゲインを
乗算した積を出力し、目標トルク生成手段１２に入力
し、関節トルク指令値の一部とする。この可変位置フィ
ードバック制御手段１３の位置偏差ゲインを調節するこ
とで、低いサーボ剛性から高いサーボ剛性まで任意に設
定でき、作業に最適な位置サーボ剛性を設定できる。

【００３８】低いサーボ剛性においても、後の関節トル
クフィードバック制御により、目標とする運動に必要な
トルクが厳密に実現されているので、高精度の軌道制御
が可能となる。

【００３９】可変速度フィードバック制御手段１４で
は、微分演算器１６より得られる目標速度と微分演算器
７より得られる現在速度との偏差を求め、この偏差に適
当なゲインを乗算した積を出力し、目標トルク生成手段
１２に入力し、関節トルク指令値の一部とする。この可
変速度フィードバック制御手段１４の速度偏差ゲインを
調節することで、低いダンピングから高いダンピングま
で任意に設定でき、作業によって最適なダンピングを設
定できる。ダンピングを調節した場合においても、後の
関節トルクフィードバック制御により、目標とするダン
ピングを実現するのに必要なトルクが厳密に実現されて
いるので、より正確なダンピング制御が可能となる。

【００４０】目標手先力生成手段１９では、マニピュレ
ータアームで発生したい目標の手先力指令値を生成す
る。この目標の手先力指令値は、手先力関節トルク変換
手段１５により関節トルク値に変換され、この関節トル
ク値を目標トルク生成手段１２に入力し、関節トルク指
令値の一部とする。手先力関節トルク変換手段１５で
は、ヤコビ行列を用いることで手先力指令値を関節トル
ク値に変換している。

【００４１】この制御系では、後の関節トルクフィード
バック制御により、目標とする手先力を実現するのに必
要な関節トルクが厳密に実現されているので、より正確
な力制御が可能となる。

【００４２】目標トルク生成手段１２では、アーム補償
トルク演算手段１０、手先力関節トルク変換手段１５、
可変位置フィードバック制御手段１３、可変速度フィー
ドバック制御手段１４より与えられる各出力の和を求
め、それを関節トルクフィードバック制御手段９の関節
トルク指令として与える。

【００４３】以上説明したように本実施例では、アーム
補償トルク指令値演算手段によって計算トルク法により
マニピュレータが望ましい運動を行うのに必要な各軸の
関節トルク指令値が厳密に計算されている。この関節ト
ルク指令値は、各軸のトルクフィードバック制御により
実現されるが、本実施例ではトルクフィードバック制御
をトルクセンサ信号だけでなく、モータ角速度も同時に
フィードバック制御するので遅れの小さいダンピングを
作用させることができ、トルク制御を安定に行うことが
できる。

【００４４】トルク制御が安定化すれば、従来に比べ
て、トルクループゲインを十分に上げることができ、指
令トルク値はより高い精度で一致し、減速機の摩擦を十
分に補償することができる。

【００４５】従って、位置フィードバックゲインが低く
ても、高い軌跡精度が得られる。また、運動に必要なト
ルク値が厳密に実現できるので、高速に動作させてもよ
り高い軌跡精度が得られる。従って、位置サーボ剛性を
低くして高速動作させても軌跡精度が向上する。

【００４６】接触作業時において低い位置サーボ剛性の
ままでワークに接近できるので、接触後、位置偏差が生
じても、過大なモータ操作力が生じない。従って、ワー
クやマニピュレータを破損することもなく、ワークに倣
って作業を行うことができるので、作業の品質、成功率
が向上する。

【００４７】モータ角速度の検出は、従来から用いられ
ているエンコーダを用いているので、新たなセンサを追
加する必要がない。

【００４８】マニピュレータの手首に力センサを装着し
ないので、手先をワーク等に衝突させても、力センサを
破損させることがない。

【００４９】マニピュレータの手先で発生したい力を各
軸のトルクに分配し、そのトルク値となるように各軸の
トルク制御を行うことで、手先での力制御を行うことが
できる。

【００５０】各軸でトルク制御を行っているので、マニ
ピュレータの手先以外の各リンクのいずれかが、障害物
等に接触しても過大な力を発生することなく、衝突した
時点で停止することができる。

【００５１】次に本発明の第２実施例を図４、図５を用
いて説明する。第２実施例においてはモータ角速度のフ
ィードバックを可変速度フィードバック制御のみを用い
て行なうことを特徴をする。

【００５２】第１実施例における現在角速度フィードバ
ックにかかるゲインＫｆｖを近似的に可変速度フィード
バック制御手段１４で用いている座標系空間でのゲイン
に換算し、可変速度フィードバック制御手段１４のゲイ
ンにあらかじめ加えておくことで、別に専用のモータ角
速度フィードバックを設ける必要がなくなり、装置が簡
略化される。

【００５３】例えば、可変速度フィードバック制御が関
節空間で行なわれているならば、本来の可変速度フィー
ドバックゲインをＫｖとした場合Ｋｖ’＝Ｋｖ＋Ｋｆｖ・・・（１）のように補正すればよい。なお、Ｋｖ’が補正された可
変速度フィードバックゲインである。

【００５４】また、可変速度フィードバック制御が行な
われる座標系が関節空間と異なる場合は、適当な座標変
換を行なってゲインを換算する必要がある。例えば、Ｋ
ｖが手先座標系空間でのゲインの場合、Ｋｖ’＝Ｋｖ＋Ｊ^-TＫｆｖＪ^-1 ・・・（２）である。ここで、Ｊはヤコビ行列である関節トルクフィ
ードバック制御手段９１における減算器９１Ａ、加算器
９１Ｆ、乗算器９１Ｂ、９１Ｃ、９１Ｄ等の働きは第１
実施例と同様である。また、減算器９Ｈを加算器９１Ｈ
に置き換える。

【００５５】本実施例においても。トルクのフィードバ
ック制御と同時にモータ角速度をネガティブフィードバ
ックするループが加えられており、より安定化すること
ができる。本実施例でも遅れのないモータ角速度を用い
てフィードバックするので、ゲインを上げることもで
き、十分に安定化できるようになる。

【００５６】本実施例では、別に専用のモータ角速度フ
ィードバックを設ける必要がなく、装置が簡略化される
という利点を持つ。

【００５７】次に本発明の第３実施例を説明する。本実
施例は、第１実施例における関節トルクフィードバック
制御手段９に代えて、図６のブロック図に示す関節トル
クフィードバック制御手段９２を用いたものである。第
３実施例の関節トルクフィードバック制御手段は、トル
クフィードバック制御を状態フィードバック法を用いて
行うことを特徴とする。

【００５８】状態フィードバック法を用いることで、望
ましい応答を極配置法等によって直接指定することがで
きる。第１および第２実施例で説明したＰＩＤ制御では
望ましい応答を得るまで試行錯誤的にゲインを調整する
必要があるが、本実施例のように状態フィードバック法
を用いれば、与えた評価関数に対して一意にゲインを決
定できる。

【００５９】図６のＫｆ１、Ｋｆ２、Ｋｆ３、Ｋｉ１が
状態フィードバックゲインである。状態フィードバック
を行うには、トルクセンサ信号およびモータ角速度信号
だけでなく、トルク値の微分値も状態量として用いる。
これはトルクセンサ信号を微分演算することで得られ
る。

【００６０】また、アーム分の角速度が得られる時はこ
れを状態量として、状態フィードバックを行うことも可
能である。

【００６１】状態フィードバックの設計は以下の手順で
行う。モータ部およびアーム部の状つを状態量に選ぶと、次式の状態方程式で表される。

【００６２】ここで、ｘ、Ａ、ｂ、ｃは次の通り定義される。

【００６３】

【数１】

【００６４】また、上記各記号は、Ｇ：減速比、Ｊｍ：
モータイナーシャ、Ｊａ：アームイナーシ、Ｋｓ：伝達
系のばね定数、Ｄｍ：モータ粘性係数、Ｄａ：アーム粘
性係数、ｕ：モータ入力トルクを各々示す。

【００６５】積分器の状態変数ｗを含めた拡大系は、次
式で表される。

【００６６】

【数２】

【００６７】ここで、τｒは関節トルク指令である。

【００６８】また、制御則は、Ｋｉ１、Ｋｆ１、Ｋｆ
２、Ｋｆ３をフィードバックゲインとして、次式で表さ
れる。

【００６９】ｕ＝Ｋｉｌｗ−ＫｘＫ＝〔Ｋｆ１Ｋｆ２Ｋｆ３〕・・・（５）状態フィードバックを行った閉ループ系の状態方程式は
次式のようになる。

【００７０】

【数３】

【００７１】そして、閉ループ系の極が設定した極とな
るように、フィードバックゲインを決定する。例えば、
閉ループ系の帯域が８Ｈｚ程度となるように、閉ループ
系の極を〔Ｚｏ −１００ −１０５ −１１０〕と設
定する。ここでＺｏはマニピュレータのモータ・アーム
系が持つ零点を相殺するためのものであり、通常−０．
５程度の値となる。この閉ループ系の極に対して、フィ
ードバックゲインＫｉ１、Ｋｆ１、Ｋｆ２、Ｋｆ３が通
常の極配置法による計算に従って決定される。ここで、
Ｋｉ１は積分ゲインを示す。

【００７２】なお、測定できる状態量はモータ角速度ω
ｍと関節トルクτであり、関節トルることが可能である。

【００７３】ここでｆｃは、カットオフ周波数であり、例えばｆｃ＝
１００Ｈｚとすることができる。

【００７４】そして、上記のようにして求められたフィ
ードバックゲインＫｉ１、Ｋｆ１、Ｋｆ２、Ｋｆ３が関
節トルクフィードバック制御手段９１の各演算器に設定
され、関節トルク指令値、現在関節トルク値、及び現在
角速度からアーム駆動分モータトルク指令値が演算され
る。

【００７５】本実施例では、トルク値の微分フィードバ
ックを同時に行なっているので、トルクフィードバック
制御自体にもダンピング要素を持たせることになり、し
たがって、より安定性を得ることができると共に、速度
フィードバックのダンピングを低く設定できるので、安
定性を保ちつつ応答性を向上することができる。

【００７６】図７に第１実施例から第３実施例をコンピ
ュータシステムによって実現するための第４実施例のハ
ードウエア構成例を示す。

【００７７】ユーザは、端末６０１を用いてコンピュー
タシステムへのデータの入出力やモニタ等を行いマニピ
ュレータを操作する。

【００７８】マニピュレータを制御するために必要な上
記で説明した第１実施例から第３実施例の何れかの計算
トルク法やトルクフィードバック制御、位置制御等の各
種演算は、ＣＰＵボード４０１によっ行なわれる。ま
た、ＣＰＵボード４０１は、システム全体の管理、ユー
ザとのインターフェイスも行う。

【００７９】ドライバーインターフェイスボード４０３
は、モータドライバ３０１およびモータユニット１０１
とＣＰＵボード４０１とのインターフェイスを行う。モ
ータドライバー３０１はモータユニット１０１を駆動、
制御する。モータユニットにはモータとモータの回転角
を検出するエンコーダ等の回転角検出手段が装着されて
おり、回転角検出手段で検出された回転角信号は、ドラ
イバーインターフェースボード４０３を介してＣＰＵボ
ード４０１に入力される。

【００８０】トルクセンサ２０１からトルク信号は、ト
ルクセンサアンプ５０１で増幅されＡ／Ｄ変換ボード４
０４を通してＣＰＵボード４０１に取り込まれる。ＣＰ
Ｕボード４０１、ドライバーインターフェイスボード４
０３、Ａ／Ｄ変換ボード４０４等はコンピュータバス４
０２に接続されたコンピュータシステムを用いることが
可能である。

【００８１】なお、本実施例では、モータドライバー３
０１、モータユニット１０１、トルクセンサ２０１、ト
ルクセンサアンプ５０１は、マニピュレータの軸数に応
じて数を増減する必要がある。

【００８２】上記第１から第４実施例はマニピュレータ
が回転関節の場合を示したものであるが、いずれかの軸
あるいは全部の軸が並進軸を含む場合は当該軸の関節ト
ルクセンサを並進力センサに置き換え、目標トルクを目
標並進力に置き換えることで、容易に回転関節の場合と
同等の働きを持つ並進力制御系を構成することができ
る。

【００８３】

【発明の効果】以上説明したように本発明によれば、計
算トルク法とトルクセンサフィードバック制御とを行う
際に、トルクセンサフィードバック制御を行うだけでな
く、モータの速度フィードバックも同時に行っているの
で、安定に、かつ高精度なトルク制御を行うことができ
る、という効果が得られる。

【図面の簡単な説明】

【図１】本発明の第１実施例を示すブロック図である。

【図２】第１実施例の関節トルクフィードバック制御手
段の詳細を示すブロック図である。

【図３】（１）は第１実施例のトルクセンサの詳細を示
す分解図である。（２）は梁構造体の平面図である。
（３）は（２）のＡ−Ａ線断面図である。

【図４】本発明の第２実施例を示すブロック図である。

【図５】第２実施例の関節トルクフィードバック制御手
段の詳細を示すブロック図である。

【図６】第３実施例の関節トルクフィードバック制御手
段の詳細を示すブロック図である。

【図７】本発明の第４実施例を示すブロック図である。

【符号の説明】

１モータ２減速機３トルクセンサ

フロントページの続き (72)発明者津坂祐司愛知県愛知郡長久手町大字長湫字横道41番地の１株式会社豊田中央研究所内 (72)発明者野村秀樹愛知県愛知郡長久手町大字長湫字横道41番地の１株式会社豊田中央研究所内 (72)発明者浅井彰司愛知県愛知郡長久手町大字長湫字横道41番地の１株式会社豊田中央研究所内 (72)発明者赤尾憲彦愛知県豊田市トヨタ町１番地トヨタ自動車株式会社内 (72)発明者佐藤義正愛知県豊田市トヨタ町１番地トヨタ自動車株式会社内

Claims

【特許請求の範囲】

【請求項１】伝達機構を介してリンクを駆動するモー
タと、モータの角度を検出する角度検出手段と、伝達機構の後の出力トルクを検出するトルク検出手段
と、目標とする運動の加速度指令値とモータの現在角度とマ
ニュピレータの慣性パラメータとに基づいて定まる慣性
分、及びモータの現在角度とモータの現在角速度とマニ
ュピレータの慣性パラメータとに基づいて定まる非線形
補償分を演算すると共に、演算された慣性分と非線形補
償分とを加算してトルク指令値を演算するトルク指令値
演算手段と、演算されたトルク指令値とトルク検出手段で検出された
現在の出力トルク値との偏差に基づいてモータのトルク
制御値を演算するトルク制御値演算手段と、演算されたトルク制御値からモータの現在角速度に基づ
く値を減算した指令値をモータに供給する供給手段と、を含むマニピュレータの制御装置。