JPH10231100A

JPH10231100A - 力補助装置のモータ制御部

Info

Publication number: JPH10231100A
Application number: JP32821997A
Authority: JP
Inventors: Akihiro Ito; 彰啓伊藤
Original assignee: Nidec Sankyo Corp
Current assignee: Nidec Sankyo Corp
Priority date: 1996-12-18
Filing date: 1997-11-28
Publication date: 1998-09-02

Abstract

(57)【要約】【課題】インピーダンス制御により人間の感覚に合致
するようアームが作動される際にアーム動作の安定性を
向上できる。【解決手段】関節部で回転可能に連結されてモータ１
８により回転されるアームを操作する操作力に関する力
・トルク情報が入力されて、この力・トルク情報を変換
すると共にアームからの角度信号をフィードバックして
モータ１８の位置サーボ制御を行う位置指令信号を出力
する力補助装置のモータ制御部において、力・トルク情
報のベクトルを速度指令に変換するインピーダンス制御
部と、速度指令を逆運動学変換により関節部の位置指令
に変換する運動学変換部と、外乱を補償する積分補償型
であり、かつアーム及び関節部の機械共振点を避ける２
自由度制御系であると共に、位置指令によりモータの位
置サーボ制御を行う位置サーボ制御部１６とを備える。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、操作者の小さい力
を補助すべく重量の大きい機器をアクチュエータにより
移動可能に支持する力補助装置に関する。更に詳述する
と、本発明は特に手術用の顕微鏡を小さい力で移動可能
に支持するのに適した力補助装置の駆動モータを制御す
るモータ制御部に関する。

【０００２】

【従来の技術】力補助装置は、高齢者又は一般人であっ
ても重さの重い物を操作者の小さい力を増幅することに
よりその物を容易に移動可能となるように支持する装置
である。そして、重量物の移動を人間の感覚に近いもの
としてスムーズな操作感を得るために、インピーダンス
制御の研究がなされている（特開平８−１９９７９号公
報参照）。そして、マニピュレータを剛体としてインピ
ーダンス制御することにより、モータのトルク指令を直
接制御している。

【０００３】ところで、医療の分野では、医者が手術中
に患部を拡大して見る顕微鏡を移動可能に支持するため
にマニピュレータスタンドによる力補助装置を使用する
ことがある。この場合の力補助装置の操作感を向上させ
るためにインピーダンス制御を行うことが望ましい。

【０００４】

【発明が解決しようとする課題】しかしながら、上述し
た顕微鏡の力補助装置では、顕微鏡の重量は相当大きい
ことと、術者に威圧感を与えないようにアーム等を極力
細くする必要があることとにより、マニピュレータスタ
ンドの固有振動数が小さくなってしまう。このため、イ
ンピーダンス制御されて人間の感覚に合致するように設
定されたアーム動作の振動数とマニピュレータスタンド
の固有振動数とが干渉してしまい、アームの動作が不安
定になることがある。また、アーム動作が不安定になら
ないようにインピーダンスの設定を行うと、アームの制
御に制約を生じてしまい、アームに所望の動作を行わせ
ることができない。

【０００５】一方、マニピュレータスタンドを剛体では
なく弾性体としてモデル化することが考えられるが、こ
れでは制御系の構造が著しく複雑になってしまう。

【０００６】そこで、本発明は、インピーダンス制御に
より人間の感覚に合致するようアームが作動される際に
アーム動作の安定性を向上できる力補助装置のモータ制
御部を提供することを目的とする。

【０００７】

【課題を解決するための手段】かかる目的を達成するた
め、請求項１の力補助装置のモータ制御部は、関節部で
回転可能に連結されてモータにより回転されるアームを
操作する操作力に関する力・トルク情報が入力されて、
この力・トルク情報を変換すると共にアームからの角度
信号をフィードバックしてモータの位置サーボ制御を行
う位置指令信号を出力する力補助装置のモータ制御部に
おいて、力・トルク情報のベクトルを速度指令に変換す
るインピーダンス制御部と、速度指令を逆運動学変換に
より関節部の位置指令に変換する運動学変換部と、外乱
を補償する積分補償型であり、かつアーム及び関節部の
機械共振点を避ける２自由度制御系であると共に、前記
位置指令により前記モータの位置サーボ制御を行う各ジ
ョイントの位置サーボ制御部とを備えるようにしてい
る。

【０００８】したがって、位置サーボ制御部はアーム及
び関節部の機械共振点を避ける２自由度制御系であるの
で、インピーダンス制御部及び運動学変換部により設定
されたインピーダンスと機械共振点とが干渉して動作を
不安定にすることはない。また、位置サーボ制御部は外
乱を補償する積分補償型であるので、重力や摩擦等の定
値外乱によるアーム動作への影響が排除される。

【０００９】そして、請求項２の力補助装置のモータ制
御部では、位置サーボ制御部は、閉ループのゲインをア
ーム及び関節部の機械共振点を避けた値に設定したフィ
ードバック制御器と、閉ループの遅れを補償するフィー
ドフォワード補償器とを備えるようにしている。

【００１０】したがって、フィードフォワード補償器に
よりフィードバック制御器の閉ループの遅れが補償され
ているので、位置サーボ制御部の全体としては入力に対
する出力の遅れを生じないことになる。すなわち、フィ
ードバック制御器は閉ループゲインをアーム及び関節部
の機械共振点を避けた値としているので、そのままでは
入力に対して出力が遅れてしまうが、フィードバック制
御器の上流側に配置されるフィードフォワード補償器に
よりフィードバック制御器での遅れの発生が予防され
る。

【００１１】このため、位置サーボ制御部では、運動学
変換部からの位置指令を入力して遅れを生ずることなく
モータの位置サーボ制御を行う。これにより、モータの
動作がインピーダンス制御部及び運動学変換部で設定し
た通りとなるので、アーム動作の設定はインピーダンス
制御部及び運動学変換部のみで独立して行うことができ
るようになる。

【００１２】

【発明の実施の形態】以下、本発明の構成を図面に示す
実施の形態の一例に基づいて詳細に説明する。図２に示
すように、本実施形態の力補助装置のモータ制御部７
は、マニピュレータスタンド８のアームを操作する操作
力に関する力・トルク情報が入力されて、この力・トル
ク情報を変換すると共にアームからの角度信号をフィー
ドバックしてアーム駆動用モータの位置サーボ制御を行
う位置指令信号を出力するものである。そして、このモ
ータ制御部７は、力・トルク情報のベクトルを速度指令
に変換するインピーダンス制御部５と、速度指令を逆運
動学変換によりアームを連結する関節部の位置指令に変
換する運動学変換部１４と、位置指令によりモータの位
置サーボ制御を行う位置サーボ制御部１６とを備えてい
る。

【００１３】さらに、位置サーボ制御部１６は、図１に
示すように、外乱を補償する積分補償型であり、かつマ
ニピュレータスタンド８の機械共振点を避ける２自由度
制御系であるものとされている。具体的には、位置サー
ボ制御部１６は、閉ループのゲインをアーム及び関節部
の機械共振点を避けた値に設定したフィードバック制御
器２１と、閉ループの遅れを補償するフィードフォワー
ド補償器２０とを備えている。

【００１４】一方、図２及び図３に示すように、力補助
装置の制御装置１は、基台９より関節部を介して順次駆
動可能に連結された剛体である複数のアームＡ１〜Ａ７
と、複数のアームＡ１〜Ａ７の各々に所定の動作をさせ
るために駆動される図示しないモータと、アームＡ７の
先端部に取り付けられた被支持機器としての手術用顕微
鏡２と、この顕微鏡２の近傍に配置される操作ハンドル
３と、操作ハンドル３に加えられる操作力を複数軸方向
の力・トルク情報として出力する力・トルクセンサ（以
下、ＦＴセンサという）４と、ＦＴセンサ４により検出
された複数軸方向の力・トルク情報をもとに各関節部Ｊ
１〜Ｊ６の動作指令を算出して、この動作指令に基づい
て複数のアームＡ１〜Ａ７を所定の方向に駆動するモー
タの制御を行うモータ制御部７とを備えている。このう
ち、モータ制御部７は、電気回路やコンピュータのソフ
トウェア等により構成されて作動される。尚、本明細書
中では「力・トルクセンサ」とは、加えられた力に対し
て所定軸、例えばＸ軸、Ｙ軸、Ｚ軸の軸方向の並進力と
各軸回りの回転力とを同時に測定可能なセンサを意味す
る。

【００１５】そして、図３〜図５に示すように、複数の
アームＡ１〜Ａ７と各アームＡ１〜Ａ７を駆動するモー
タとによりマニピュレータスタンド８が構成されてい
る。このマニピュレータスタンド８は、基台９に支持さ
れた上下方向に移動可能な第１アームＡ１と、該第１ア
ームＡ１に対して鉛直線Ｃ１を中心に水平方向に回転可
能に取り付けられた第２アームＡ２と、これら第１アー
ムＡ１及び第２アームＡ２を連結する第１関節部Ｊ１
と、第２アームＡ２に対して水平軸線Ｃ２，Ｃ２を中心
に鉛直方向に揺動可能に取り付けられた１組の平行リン
クから成る第３アームＡ３と、これら第２アームＡ２及
び第３アームＡ３を連結する第２関節部Ｊ２と、第３ア
ームＡ３に対して水平軸Ｃ３を中心に揺動可能に取り付
けられた第４アームＡ４と、これら第３アームＡ３及び
第４アームＡ４を連結する第３関節部Ｊ３と、第４アー
ムＡ４に対して鉛直軸Ｃ４を中心に水平方向に回転可能
に取り付けられた第５アームＡ５と、これら第４アーム
Ａ４及び第５アームＡ５を連結する第４関節部Ｊ４と、
第５アームＡ５に対して鉛直軸Ｃ５を中心に水平方向に
回転可能に取り付けられた第６アームＡ６と、これら第
５アームＡ５及び第６アームＡ６を連結する第５関節部
Ｊ５と、第６アームＡ６に対して水平軸Ｃ６，Ｃ６’を
中心に回転可能に取り付けられると共に顕微鏡２や操作
ハンドル３が取り付けられた第７アームＡ７と、これら
第６アームＡ６及び第７アームＡ７を連結する第６関節
部Ｊ６とを備えている。

【００１６】また、各関節部Ｊ１〜Ｊ６には、それぞれ
独立して回転するモータが取り付けられている。ここで
のモータとしては、電力を駆動源とするサーボモータ若
しくはステッピングモータを用いたり、また油圧モータ
を用いることができる。さらに、各関節部Ｊ１〜Ｊ６に
は、図示しない角度センサが取り付けられている。この
角度センサにより各関節部Ｊ１〜Ｊ６の現在の角度が検
出され、この検出結果から顕微鏡２の位置及び移動時の
姿勢が算出される。角度センサとしては例えばエンコー
ダを使用することができる。

【００１７】第１アームＡ１は、本実施形態では基台９
に対して昇降及び水平方向に回転可能に取り付けられて
いる。但し、これに限らず基台９に固定しても構わな
い。各アームのそれぞれを固定状態とするか、水平又は
上下方向移動とするか、水平又は垂直方向回動とするか
等、どのように各アームを連結するかは、どのように被
支持機器を支持し動かすかによって定めるべき事項であ
り、適宜設定すればよい内容であるので、他の構成例に
ついては細かく説明しない。また、第３アームＡ３，Ａ
３は１組の平行リンク機構を構成しており、各アームＡ
３，Ａ３の基端側の揺動中心線Ｃ２，Ｃ２は第２アーム
Ａ２の鉛直方向の位置に配置され、各アームＡ３，Ａ３
の先端側の揺動中心線Ｃ３，Ｃ３は第４アームＡ４の鉛
直方向の位置に配置されている。したがって、第４アー
ムＡ４は鉛直方向の姿勢が変わらずに移動可能となる。

【００１８】顕微鏡２としては、光学顕微鏡や撮像素子
を有するビデオカメラ等を使用することができる。そし
て、本実施形態では、マニピュレータスタンド８による
顕微鏡２の移動及び回転は、指定された移動モードとし
て操作者により選択されたポイントロックモードか平行
移動モードかフォーカス移動モードかオールフリーモー
ドかのいずれかのモードにより行われる。

【００１９】ポイントロックモードは、図８に３位置を
代表例として示すように、顕微鏡２の焦点位置１０を固
定しその固定した焦点位置１０を中心にして焦点距離を
半径とする球面上で、顕微鏡２，２’，２”が常に焦点
位置１０を向くように移動及び回転可能に制御するモー
ドである。このモードは、顕微鏡２により焦点位置１０
を様々な角度から見るときに使用される。

【００２０】平行移動モードは、図９に３位置を代表例
として示すように、顕微鏡２，２’，２”のＸ，Ｙ，Ｚ
軸の軸方向の並進のみを許容し、回転動作を制限するモ
ードである。この場合、焦点位置１０，１０’，１０”
も顕微鏡２の移動に伴って移動する。このモードは、手
術中の微調整時等に顕微鏡２の鏡筒の姿勢を変えたくな
い場合等に使用される。

【００２１】フォーカス移動モードは、図１０に示すよ
うに、顕微鏡２の鏡筒、即ち焦点軸（つまりＺ軸）方向
の並進とＺ軸回りの回転のみが許容されるモードであ
る。このモードは、合焦動作に使用される。

【００２２】オールフリーモードは、顕微鏡２のＸ，
Ｙ，Ｚ軸の軸方向の並進及び回転の６自由度の全てを自
由に変更できるモードである。このモードは、顕微鏡２
の鏡胴や焦点を自由に移動できるので手術を行う際のお
おざっぱな位置決め等に使用される。

【００２３】なお、顕微鏡２の移動モードとしては、こ
れらのモードに限られない。例えば、顕微鏡２のＸ，Ｙ
軸の軸方向の並進のみを許容し、Ｚ軸方向の並進や各回
転を制限する平面移動モードを備えても構わない。この
場合、焦点位置１０はＸＹ平面内で移動する。

【００２４】また、操作ハンドル３は、ＦＴセンサ４を
介して第７アームＡ７に取り付けられている。操作ハン
ドル３は、図６及び図７に示すように、全体としてほぼ
山形の形状をなし、中央の取付部３ａと該取付部３ａの
両端部を斜めに曲折した形状の把持部３ｂ，３ｂとを備
えている。左右の把持部３ｂ，３ｂの開放端部近傍に
は、それぞれ２個のプッシュスイッチＳＬ１，ＳＬ２，
ＳＲ１，ＳＲ２が配置されている。これらのプッシュス
イッチＳＬ１，ＳＬ２，ＳＲ１，ＳＲ２の操作により、
顕微鏡２の移動及び回転の所定モード、即ち移動方向を
指定する移動モードが設定される。

【００２５】左上スイッチＳＬ１と右上スイッチＳＲ１
とは、顕微鏡２の移動及び回転のモードを設定する場合
に同じ機能を果たす。また、左下スイッチＳＬ２と右下
スイッチＳＲ２とも、顕微鏡２の移動及び回転のモード
を設定する場合に同じ機能を果たす。すなわち、顕微鏡
２の移動及び回転のモード設定する場合、左手により左
側のスイッチＳＬ１，ＳＬ２のみを操作しても、右手に
より右側のスイッチＳＲ１，ＳＲ２のみを操作しても、
両手により両側のスイッチＳＬ１，ＳＬ２，ＳＲ１，Ｓ
Ｒ２を操作しても同じモード設定とみなされる。但し、
右手による操作と左手による操作とが不一致の場合は動
作せず、操作者に報知される。

【００２６】そして、本実施形態では、少なくとも一方
の上側スイッチＳＬ１又はＳＲ１のオンによりポイント
ロックモードとなり、少なくとも一方の下側スイッチＳ
Ｌ２又はＳＲ２のオンにより平行移動モードとなり、少
なくとも１組の上側及び下側のスイッチＳＬ１及びＳＬ
２またはＳＲ１及びＳＲ２のオンによりオールフリーモ
ードとなるようスイッチの割り付けがなされ、片手で操
作ができるモードと両手でなければ操作できないモード
とを区別している。

【００２７】また、全てのスイッチをオフしている状態
では、各関節部Ｊ１〜Ｊ６がロックされることにより顕
微鏡２の位置及び姿勢がロックされ動かない。このた
め、不要な力が操作ハンドル３に加わっても顕微鏡２の
位置がずれることはない。なお、図３及び図４中の符号
３２は非常停止スイッチであり、この非常停止スイッチ
の操作によっても各関節部Ｊ１〜Ｊ６がロックされる。

【００２８】本実施形態では、顕微鏡２の移動及び回転
のモードをポイントロックモードか平行移動モードかオ
ールフリーモードかのいずれかとしているが、これに限
らず例えばスイッチを増設したり割り付けを変更する等
してフォーカス移動モードや更に別のモードを含めても
構わない。

【００２９】また、各スイッチＳＬ１，ＳＬ２，ＳＲ
１，ＳＲ２としては、いずれもボタンを押圧していると
きのみオンされ、指を離すとボタンが戻ってオフになる
種類のスイッチが使用されている。但し、スイッチの割
り付けやスイッチの種類は、これらに限られないのは勿
論である。さらに、本実施形態では左右の把持部３ｂ，
３ｂに２個ずつのスイッチを配置しているが、これには
限られず、例えば、スイッチを左右の把持部３ｂ，３ｂ
に１個ずつ配置して、スイッチをオン（クリック動作）
した回数によりモードを切り換えるようにしても良い。

【００３０】ＦＴセンサ４としては、本実施形態では円
盤形状の市販品（フォーストルクセンサ、ビーエルオー
トテック社製、又は６軸力覚センサ、ニッタ社製）の６
軸フォーストルクセンサが使用されている。この６軸フ
ォーストルクセンサは、Ｘ，Ｙ，Ｚの各軸方向である３
方向の並進力及びＸ，Ｙ，Ｚの各軸回りの３方向のトル
クを検出するもので、基体に歪みゲージを張り付け、そ
の歪みゲージの出力信号を処理することで、上記６軸方
向の力・トルク情報である信号を得ることができるセン
サである。このＦＴセンサ４は、図７に示すように、Ｆ
Ｔセンサ４の内プレート４ａには操作ハンドル３の取付
部３ａが固定され、ＦＴセンサ４の外筒４ｂは第７アー
ムＡ７に止着されている。このＦＴセンサ４は、内蔵さ
れた複数の歪みゲージにより操作ハンドル３への操作者
の操作力またはトルクを６軸方向について検出する。

【００３１】図２に示すように、ＦＴセンサ４とインピ
ーダンス制御部５との間には、ＦＴ（力・トルク）セン
サアンプ１１とＦＴ（力・トルク）センサ信号処理部１
２と操作力座標変換部１３とが介在されている。これら
ＦＴセンサアンプ１１とＦＴセンサ信号処理部１２と操
作力座標変換部１３とは、電気回路やコンピュータのソ
フトウェア等により構成され作動される。

【００３２】ＦＴセンサ４には、ＦＴセンサアンプ１１
が接続されている。このＦＴセンサアンプ１１は、ＦＴ
センサ４からの力・トルク情報である上記６軸方向のア
ナログ信号を増幅してデジタル信号に変換する。

【００３３】ＦＴセンサアンプ１１には、ＦＴセンサ信
号処理部１２が接続されている。ＦＴセンサ信号処理部
１２は、ＦＴセンサアンプ１１からのデジタル信号をフ
ィルタに通過させて高周波成分をノイズカットすると共
にＦＴセンサ４の固有の補正値に基づいて較正する。す
なわち、ＦＴセンサ４は通常、固有のバイアス等を持っ
ているので、このような測定値を較正して正しい６軸方
向の力またはトルクに変換する。

【００３４】ＦＴセンサ信号処理部１２には、操作力座
標変換部１３が接続されている。操作力座標変換部１３
は、操作ハンドル３の操作方向とＦＴセンサ４の取付方
向とのずれを補正するためにＦＴセンサ信号処理部１２
で得られた６軸方向の力またはトルクの座標変換を行
う。すなわち、ＦＴセンサ４を第７アームＡ７に取り付
ける際に取付スペースの都合により、ＦＴセンサ４が検
知する６軸方向の座標とマニピュレータスタンド８の６
軸方向の座標とが一致しないことがある。この場合にＦ
Ｔセンサ４が測定した６軸方向の座標を操作力座標変換
部１３により補正してマニピュレータスタンド８の座標
と一致させるものである。

【００３５】操作力座標変換部１３では、操作ハンドル
３の重力補償に関する補正をも行うことが好ましい。そ
の方法は公知のもので構わない。

【００３６】また、本実施形態では、ＦＴセンサ４とモ
ータ制御部７との間にＦＴセンサアンプ１１とＦＴセン
サ信号処理部１２と操作力座標変換部１３とが介在され
ているが、これに限らずＦＴセンサ４による測定値が６
軸の力またはトルクに対応し操作力との間で座標が一致
するものであれば、これらＦＴセンサアンプ１１とＦＴ
センサ信号処理部１２と操作力座標変換部１３とを設け
なくても構わない。

【００３７】さらに、図２に示すように、操作力座標変
換部１３には、モータ制御部７が接続されている。モー
タ制御部７は、インピーダンス制御部５と軌道制御部６
と運動学変換部１４と位置指令変換部１５と位置サーボ
制御部１６とサーボドライバ１７とを有している。

【００３８】操作力座標変換部１３に接続されるインピ
ーダンス制御部５では、インピーダンス制御が行われ
る。一般的には、インピーダンス制御とは、アクチュエ
ータをトルク発生装置として数式１によりアクチュエー
タの操作力Ｔｍを制御する方法である。

【００３９】

【数１】Ｔｍ＝（Ｊ−Ｊｎ）θ”＋（Ｃ−Ｃｎ）θ’＋Ｋｎ（θｒ−θ） θ”＝（１／Ｊｎ）｛Ｋｎ（θｒ−θ）−Ｃｎθ’＋Ｔｄ｝本実施形態では、インピーダンス制御部５は、操作者が
操作ハンドル３を操作する際に重力負荷や弾性的な振動
を感ずることなく慣性または粘性を感じながら操作でき
るように、６軸方向の力またはトルクを慣性または粘性
を感じられる操作感を得られるような各軸方向の速度ま
たは角速度の指令ベクトルに変換する。したがって、顕
微鏡２の操作者はより自然な動作に近いアナログ的な操
作感を得ることができ、肉体的・精神的な負担が軽減さ
れる。

【００４０】慣性または粘性を感じられる操作感を得ら
れるようなインピーダンス制御部５での変換の方法とし
ては、公知の方法を利用することができる。例えば、慣
性及び粘性の一般的な運動方程式である数式２を数式３
のように変形して、ＦＴセンサ４により検出された力・
トルクベクトルｆから速度ベクトルｖを得る。

【００４１】

【数２】Ｍｖ’＋Ｃｖ＝ｆ但し、Ｍ：慣性係数行列、Ｃ：粘性係数行列

【００４２】

【数３】ｖ＝（Ｍｓ＋Ｃ）^-1ｆここで、ｖ＝（ＭｘＭｙＭｚＲｘＲｙＲｚ）^T 但し、Ｍｘ，Ｍｙ，Ｍｚは焦点のＸ，Ｙ，Ｚ軸方向の並
進速度指令を表し、Ｒｘ，Ｒｙ，Ｒｚは焦点のＸ，Ｙ，
Ｚ軸回りの回転角速度指令を表す。

【００４３】したがって、操作点３ｃでの力・トルク信
号の各軸成分がインピーダンス制御により各軸方向の動
作指令である速度指令に変換され求められる。また、慣
性係数行列Ｍや粘性係数行列Ｃの大きさは通常固定で良
いが、これらの大きさを可変として操作性を調整可能に
することが好ましい。

【００４４】さらに、インピーダンス制御部５では、操
作ハンドル３を両手で操作するときには大きな操作ゲイ
ンにより顕微鏡２の移動または回転の速度が比較的速
く、片手で操作するときには小さな操作ゲインにより顕
微鏡２の速度が比較的遅くなるように設定されている。
これにより、顕微鏡２の移動・回転について速度性又は
精緻性のいずれを優先するかが顕微鏡２の使用状態、即
ち両手で操作するか片手で操作するかに応じて選択する
ことができる。これは、両手で操作するときは大まかな
位置決めを行うことが多いので速い動作が要求されるこ
とと、片手で操作するときは手術中に顕微鏡２を覗いて
いることが多いので微少な位置設定を行うために精緻性
が要求されることとによるものである。

【００４５】インピーダンス制御部５には、軌道制御部
６が接続されている。軌道制御部６は、操作スイッチＳ
Ｌ１，ＳＬ２，ＳＲ１，ＳＲ２により選択された顕微鏡
２の移動モードに基づいて顕微鏡２が移動可能な軌道を
設定すると共に、その軌道上で操作ハンドル３の操作に
対応した顕微鏡２の移動及び回転の方向を決定する。す
なわち、軌道制御部６では、インピーダンス制御部５で
算出された移動速度指令ベクトルを各移動モードに合致
する動作指令に変換する。

【００４６】各移動モードにおける動作指令としての顕
微鏡２の移動及び回転の動作速度指令ベクトルの変換方
法を以下に説明する。

【００４７】固定された第１アームＡ１の座標系を原座
標系とし、顕微鏡２の焦点位置１０の座標系を焦点座標
系とした場合に、この焦点座標系の原座標系に対する同
時変換行列Ｔｆが公知の方法により求める。例えば、ベ
クトル（△ｘ △ｙ △ｚθｘ θｙ θｚ）^Tに対す
る同時変換行列Ｔｆは、数式４に示すものとなる。そし
て、各モード固有の既知の回転行列Ｒｕを基にして、数
式５に示すように新たな同時変換行列Ｔｆ’を求める。

【００４８】

【数４】

【００４９】

【数５】Ｔｆ’＝Ｔｆ・Ｒｕ例えばポイントロックモードでは、顕微鏡２の焦点位置
１０を中心に焦点距離を一定に保ちながら顕微鏡２の位
置と姿勢を一定球面上のみの移動として許す動作である
から、同時変換行列Ｔｆの位置ベクトル（ｕｖｗ）
を変更せずに姿勢ベクトル（θｕ θｖ θｗ）のみを
変更する動作を行う。具体的には、焦点座標系のｕ，
ｖ，ｗ軸回りの回転θｕ，θｖ，θｗのみを行う。例え
ば、ｕ軸回りにθｕだけ回転させる際の回転行列Ｒｕ
は、数式６に示すものとなる。

【００５０】

【数６】

【００５１】そして、インピーダンス制御部５で算出さ
れた動作速度指令ベクトルを同時変換行列Ｔｆ’により
変換する。その際に、動作速度指令ベクトルの各要素の
うち、移動モードにより異なる所定の要素のみを使用す
る。

【００５２】オールフリーモードでは、Ｘ，Ｙ，Ｚ各軸
方向の並進及び回転（姿勢変更）の６自由度全てを自由
に動作できるモードであるから、焦点の動作速度指令ベ
クトルは数式７に示すものとなる。

【００５３】

【数７】（ＭｘＭｙＭｚＲｘＲｙＲｚ）^T この動作速度指令ベクトルのＭｘ，Ｍｙ，Ｍｚは焦点の
Ｘ，Ｙ，Ｚ軸方向の並進速度指令を表し、Ｒｘ，Ｒｙ，
Ｒｚは焦点のＸ，Ｙ，Ｚ軸回りの回転角速度指令を表
す。したがって、このモードではインピーダンス制御部
５で算出された動作速度指令ベクトルをそのまま変換す
ることになる。

【００５４】ポイントロックモードでは、焦点位置１０
が固定され焦点回りの回転のみが許容されるので、動作
速度指令ベクトルは数式８に示すものとなる。

【００５５】

【数８】（０００Ｒｘ＋αＭｙＲｙ＋αＭｘＲｚ）^T したがって、インピーダンス制御部５で算出された動作
速度指令ベクトルのうち、焦点のＸ，Ｙ，Ｚ軸方向の並
進速度指令を表すＭｘ，Ｍｙ，Ｍｚの各要素は位置ベク
トルとしては無視されるので、操作力として軸方向への
並進力が加えられても顕微鏡２がその方向に移動してし
まうことはない。なお、焦点回りの球面上で移動を行う
ときに操作ハンドル３の操作点３ｃが僅かに並進してい
ることから、Ｒｘ及びＲｙについて並進を考慮した補正
がなされている。

【００５６】平行移動モードでは、動作速度指令ベクト
ルは数式９に示すものとなる。

【００５７】

【数９】（ＭｘＭｙＭｚ０００）^T したがって、操作点３ｃでの各軸回りの回転指令は無効
となり、顕微鏡の向きは維持される。

【００５８】フォーカス移動モードでは、動作速度指令
ベクトルは数式１０に示すものとなる。

【００５９】

【数１０】（００Ｍｚ００Ｒｚ）^T したがって、顕微鏡２の鏡筒軸の軸方向の並進と該軸回
りの回転のみが許容される。

【００６０】なお、各モードにおける顕微鏡２の移動及
び回転の動作速度指令ベクトルの変換方法は上述した本
実施形態のような方法に限らず、他の方法でも構わない
のは勿論である。

【００６１】さらに、運動学変換部１４は、軌道制御部
６により算出された動作速度指令ベクトルに従って顕微
鏡２を動作させる際の各関節部Ｊ１〜Ｊ６を駆動するた
めのモータの角速度を算出する。各モータの角速度の算
出は、角度センサにより得られた各関節部Ｊ１〜Ｊ６の
角度から顕微鏡２の位置及び向きを求め、この顕微鏡２
を前述の軌道制御部６により算出された動作速度指令ベ
クトルに従って動作させるように逆運動学変換により行
われ各モータの位置（角度）指令を出力する。

【００６２】そして、位置指令変換部１５では、各モー
タの角位置指令を補間して、各モータの位置サーボ系に
同期した位置（角度）指令を生成する。すなわち、位置
指令変換部１５は運動学変換部１４による各モータの角
位置を補間する。このため、運動学変換部１４で十分に
細かく各モータの角位置の算出がなされていれば、位置
指令変換部１５はなくても構わない。

【００６３】また、図１及び図２に示すように、位置サ
ーボ制御部１６と例えばＤ／Ａコンバータから成るサー
ボドライバ１７とモータ１８と該モータ１８による回転
位置を検出するための角度センサとしてのサンプラー１
９とは、モータ１８の角度がサンプラー１９により角度
信号として検出され、この検出出力が位置サーボ制御部
１６にフィードバックされる位置（角度）サーボ制御系
とされている。そして、マニピュレータスタンド８の所
定箇所に取り付けられた各関節部Ｊ１〜Ｊ６を駆動する
各モータは、それぞれこのような位置（角度）サーボ制
御系によって制御される。

【００６４】この位置（角度）サーボ制御系の中で、位
置サーボ制御部１６は、図１に示すように、外乱を補償
する積分補償型であり、かつマニピュレータスタンド８
の機械共振点を避ける２自由度制御系であるものとされ
ている。具体的には、位置サーボ制御部１６は、閉ルー
プのゲインをアーム及び関節部の機械共振点を避けた値
に設定したフィードバック制御器２１と、閉ループの遅
れを補償するフィードフォワード補償器２０とを備えて
いる。すなわち、各関節部Ｊ１〜Ｊ６ごとの位置サーボ
系を、積分補償型の２自由度制御系として構成してい
る。

【００６５】また、サーボドライバ１７はＰＷＭインバ
ータ等による電力変換器であり、位置サーボ制御部１６
から出力された位置指令信号をモータ駆動用の電力に変
換してモータ１８に供給する。このサーボドライバ１７
としては、電流フィードバック補償を含んだものを使用
しても構わない。また、モータ１８としてＡＣモータを
使用する場合は、コミュテーション制御を行うことが望
ましい。

【００６６】そして、位置指令変換部１５からの位置指
令をフィードフォワード補償器２０及びフィードバック
制御器２１で連続的に処理する。そして、得られた位置
指令をサーボドライバ１７に入力してモータ１８を駆動
する。モータ１８の駆動による各関節部Ｊ１〜Ｊ６の角
度は角度センサとしてのサンプラー１９により検出さ
れ、フィードバック制御器２１の内部でフィードバック
される。

【００６７】フィードバック制御器２１は、モータ制御
における速度推定機能と、その推定値による極配置機能
と、積分補償による外乱推定補償機能とを有するものと
されている。このフィードバック制御器２１の制御特性
は、図１１のボード線図の太実線に示すように、第１ポ
ール及び第２ポールで折れ曲がり、各曲折点の前後は高
周波を除きほぼ直線状となる。また、フィードバック制
御器２１の制御特性は、マニピュレータスタンド８の共
振点を避けるように低く設定されている。

【００６８】このフィードバック制御器２１の速度推定
機能と極配置機能とは、以下の理論により導かれる。ま
ず、モータ１８の位置出力の伝達関数はＫ／｛ｓ（ｓ＋
λ）｝により近似される。この伝達関数をデジタル制御
するためにサーボドライバ１７による０次ホールドとサ
ンプラー１９による位置のサンプルとを付加してサンプ
リング時間τ（秒）で制御することにすると、サーボド
ライバ１７からサンプラー１９までの間での離散時間系
の入出力特性は、差分演算子δを用いて数式１１により
近似表現される。

【００６９】

【数１１】ｙ_i＝｛ｒ₀／ｐ（δ）｝ｕ_i ｐ（δ）＝δ²＋ｐ₁δ ここで、ｙ_iは位置のサンプル出力、ｕ_iはサーボドラ
イバ１７への制御入力である。このフィードバック制御
器２１の目的は、位置指令ｖ_iからサンプル出力ｙ_i に
至る伝達関数を、数式１１の入出力特性に対して数式１
２に示すものとなるように制御することである。

【００７０】

【数１２】ｙ_i＝｛ｍ₀／ｍ（δ）｝ｖ_i ｍ（δ）＝δ²＋ｍ₁δ＋ｍ₀ ここで、ｍ₀／ｍ（δ）は、ω₁及びω₂（ｒａｄ／
ｓ）に極を持つように設定される。そして、数式１２を
満たすように、フィードバック制御器２１を数式１３に
示す制御則に沿って構成する。

【００７１】

【数１３】

【００７２】数式１３をブロック図化したものを図１
（Ｂ）に示す。同図に示すように、フィードバック制御
器２１は、比例制御部２２と１次制御部２３と２次制御
部２４と２次ＩＩＲフィルタ２５とを備えることにな
る。比例制御部２２は、位置指令ｖ_iにＧを掛けてｕ_i
としてサーボドライバ１７に出力する。１次制御部２３
は、ｕ_i にｋ（δ）を掛けて出力する。２次制御部２４
は、サンプラー１９からのサンプル出力ｙ_i にｈ（δ）
を掛けて出力する。２次ＩＩＲフィルタ２５は、１次制
御部２３と２次制御部２４とから出力の和をｑ（δ）で
除してｕ_i に加える。

【００７３】ここで、ｑ（δ），ｋ（δ），ｈ（δ）は
δの多項式で、数式１４に示すものとされる。ここで、
ｑ（δ）は任意に設定するカットオフ周波数ω_q（ｒａ
ｄ／ｓ）の２次ＩＩＲフィルタの分母である。

【００７４】

【数１４】ｑ（δ）＝δ²＋ｑ₁δ＋ｑ₀ ｋ（δ）＝ｋ₁δ＋ｑ₀ ｈ（δ）＝ｈ₂δ²＋ｈ₁δ＋ｈ₀ 次に、数式１４の係数ｋ₁ ，ｈ₀ ，ｈ₁ ，ｈ₂を求め
る。Ｇ＝ｍ₀ ／ｒ₀ とすると、数式１１及び数式１３に
より位置指令ｖ_iからサンプル出力ｙ_i に至る特性は、
数式１５に示すようになる。

【００７５】

【数１５】

【００７６】ここで、数式１６を満たすように係数ｋ
₁ ，ｈ₀ ，ｈ₁ ，ｈ₂を求めることにより、制御目的が
達成される。

【００７７】

【数１６】ｐ（δ）｛ｑ（δ）−ｋ（δ）｝−ｒ₀ｈ
（δ）＝ｍ（δ）ｑ（δ）この数式１６は両辺ともδについての４次のモニック多
項式であるから、係数比較によって４本の方程式が連立
することになる。これに対し未知係数は４つであるの
で、いずれも唯一解が求められる。

【００７８】一方、フィードバック制御器２１の外乱推
定補償機能は積分補償によるものであり、以下の理論に
より導かれる。この外乱推定補償機能とは、外乱に対す
る応答特性、即ち制御入力ｕ_i に加わる外乱ξ_i からサ
ンプル出力ｙ_iへの応答特性である。また、外乱として
は、重力や摩擦等の定値外乱が考えられる。

【００７９】制御対象に対しては数式１７に示すように
外乱が加わる。

【００８０】

【数１７】ｙ_i＝｛ｒ₀／ｐ（δ）｝（ｕ_i＋ξ_i）ここで、数式１７のｕ_i に制御則（数式１３）を代入し
てｖ_i ＝０として整理すると数式１８に示すようにな
る。

【００８１】

【数１８】

【００８２】ここで、外乱ξ_i をステップ状定値外乱と
すると、そのｚ変換はξ’／δのように表される。そし
て、数式１９の関係が成り立つので、数式１８の定常応
答は０となる。

【００８３】

【数１９】ｑ（δ）−ｋ（δ）＝δ（δ＋ｑ₁−ｋ₁）したがって、フィードバック制御器２１が外乱ξ_i を抑
制することができる。特に、重力による外乱は完全に補
償されることになるので、重力に対する位置オフセット
を生ずることがないと共に各関節部Ｊ１〜Ｊ６の位置サ
ーボ系が安定する。

【００８４】一方、このフィードバック制御器２１の制
御特性は、図１１のボード線図の太実線に示すように第
１ポール及び第２ポールで折れ曲がった形状であるの
で、このままでは位置指令変換部１５からの位置指令に
対する応答に遅れを生じてしまう。このため、位置指令
変換部１５とフィードバック制御器２１との間に設けら
れたフィードフォワード補償器２０により、閉ループ系
であるフィードバック制御器２１の極を補償する。これ
により、デッドビート特性が生じて、位置指令に対する
応答性が向上する。以下、フィードフォワード補償の理
論を説明する。

【００８５】低次元ロバスト極配置法により制御系を設
計した場合、閉ループ制御系の離散時間系伝達関数は数
式２０に示すものとなる。

【００８６】

【数２０】

【００８７】ここで、τは制御周期（秒）であり、
ｍ₁，ｍ₂はそれぞれ閉ループ制御系の極（実根）（ｒ
ａｄ／ｓ）である。そして、フィードフォワード補償器
２０の目的は、この閉ループ制御系にフィードフォワー
ド補償を行い、指令に対する応答特性及び追従性を改善
することである。

【００８８】図１２に示すように、出力が有限整定応答
となるようなフィードフォワード補償を掛けた場合は、
指令応答は（ｚ^-2）となりオーバーシュートのない良好
なデッドビート特性が得られる。このときの位置偏差応
答は（１−ｚ^-2）であるので、定位置定常偏差（ステッ
プ応答偏差）は、数式２１に示すように０となる。

【００８９】

【数２１】

【００９０】これに対し、定速度定常偏差（ランプ応答
偏差）は、数式２２に示すように２τとなる。すなわ
ち、周波数Ｆ（Ｈｚ）の２τ倍の位置偏差が残ることに
なる。

【００９１】

【数２２】

【００９２】そして、フィードフォワード補償の実現は
数式２３に示すようにする。

【００９３】

【数２３】ｖ_k＝Ｖ_k-2＋（Ｖ_k−Ｖ_k-1）／ａ₀＋（ａ₁−
１）（Ｖ_k-1−Ｖ_k-2）／ａ₀ ここで、Ｖ_kは位置指令であり、ｖ_kはフィードフォワ
ード補償の出力である。数式２３をブロック図化したも
のを図１３に示す。同図に示すように、フィードフォワ
ード補償器２０は、第１〜第６制御部２６，２７，２
８，２９，３０，３１を備えたものとなる。第１制御部
２６は、位置指令Ｖ_kにｚ^-1を掛けて出力する。第２制
御部２７は、第１制御部２６からの出力にｚ^-1を掛けて
出力してフィードフォワード補償の出力ｖ_kとする。第
３制御部２８及び第４制御部２９は、位置指令Ｖ_kと第
１制御部２６の出力との差を入力する。第３制御部２８
では、入力値にＦ₁ （＝１／ｍ₁τ）を掛けて第１制御
部２６の出力に加える。第４制御部２９では、入力値に
ｚ^-1を掛けて出力する。第５制御部３０は、第３制御部
２８の出力ｚ^-1を掛けて出力する。第６制御部３１は、
第３制御部２８及び第４制御部２９の出力の和と第５制
御部３０の出力との差を入力してＦ₂（＝１／ｍ₂τ）
を掛けて第２制御部２７の出力に加える。

【００９４】以上により構成した力補助装置の制御装置
の作動を図１４に示すフローチャートに沿って説明す
る。操作者が操作ハンドル３に設けられたプッシュスイ
ッチＳＬ１，ＳＬ２，ＳＲ１，ＳＲ２を押して所望の移
動モードを選択しながら操作ハンドル３を操作すると、
その操作によって操作ハンドル３に加えられた操作力は
ＦＴセンサ４により検出される。ＦＴセンサ４は、操作
ハンドル３に加えられた操作力を受けて、その操作力の
力又はトルクを６軸方向のＦＴセンサ信号として出力す
る（ステップ１０１）。ＦＴセンサ信号はＦＴセンサア
ンプ１１及びＦＴセンサ信号処理部１２で処理されて、
ＦＴセンサ４における６軸方向の力・トルクに関するデ
ジタル信号に変換される（ステップ１０２，ステップ１
０３）。

【００９５】一方、ハンドル操作を行う時と同時に操作
されたプッシュスイッチ等の操作スイッチＳＬ１，ＳＬ
２，ＳＲ１，ＳＲ２の指示により、操作者が選択した顕
微鏡２の移動モードに対応した操作スイッチ信号が発生
する（ステップ１０４）。操作スイッチ信号は、操作力
座標変換部１３とインピーダンス制御部５と軌道制御部
６とに伝達される。操作力座標変換部１３では、ハンド
ル３の操作が両手か片手かにより座標変換方法を異なら
せるようになっている。

【００９６】そして、操作力座標変換部１３によりＦＴ
センサ４の取付けと顕微鏡２の関係を補正されて顕微鏡
の向きに合致した方向の力とトルクの信号に変換される
（ステップ１０５）。この時、ＦＴセンサ４の取り付け
と顕微鏡２の関係を補正して顕微鏡２の向きに合致した
方向の力とトルクの信号に変換する。変換された信号は
インピーダンス制御部５によりインピーダンス制御され
て、操作者に慣性と粘性とを感じさせる速度指令に変換
される（ステップ１０６）。さらに、軌道制御部６で
は、操作者が操作スイッチＳＬ１，ＳＬ２，ＳＲ１，Ｓ
Ｒ２により設定した移動モードに応じて所定の軌道に沿
って顕微鏡２が移動するように、速度指令が選択または
変換される（ステップ１０７）。

【００９７】一方、各関節部Ｊ１〜Ｊ６を駆動する各モ
ータの回転角度は各モータに連結されたエンコーダによ
り検出されている（ステップ１１３）。このエンコーダ
により検出される角度信号は、運動学変換部１４と位置
サーボ制御部１６とサーボドライバ１７とに入力され
る。

【００９８】軌道制御された各モータ用の速度指令は運
動学変換部１４で逆運動学変換され、各モータ（各軸）
を駆動するための角位置指令に変換される（ステップ１
０８）。この角位置指令は位置指令変換部１５で位置サ
ーボ制御部１６に同期するよう補間される（ステップ１
０９）。そして、位置サーボ制御部１６及びサーボドラ
イバ１７を介して各モータ（各軸）ごとにモータ駆動の
フィードバック制御及びフィードバック制御を行う（ス
テップ１１０〜ステップ１１２）。このように、アーム
Ａ１〜Ａ７の移動及び回転を行うための各モータを制御
することにより、操作者は顕微鏡２に所望の移動または
回転を与えることができる。

【００９９】したがって、本実施形態の力補助装置の制
御装置によれば、操作力座標変換部１３でＦＴセンサ４
の座標系を顕微鏡２の焦点座標系に変換する際に、操作
スイッチＳＬ１，ＳＬ２，ＳＲ１，ＳＲ２の操作状態に
応じた補正をも行っているので、操作ハンドル３の操作
を両手で行っても片手のみで行っても自然な操作感を得
ることができる。

【０１００】また、図１（Ｂ）に示すように、位置サー
ボ制御部１６のフィードバック制御器２１は、オブザー
バフィードバックによる極配置制御系に積分型の外乱補
償機能を持たせると共に制御系のモデル次数を２次とい
う少ない次数に抑えながら外乱ロバストとなる完全極配
置を達成している。

【０１０１】そして、このフィードバック制御器２１を
使用することにより、比較的簡単な構造のフィードフォ
ワード補償器２０で指令応答特性を有限整定応答に近似
させること、即ち約１００％のフィードフォワード効果
を得ることができる。これにより、インピーダンス制御
により算出された好ましい操作感を損なうことなく顕微
鏡２の移動・回転に反映することができる。

【０１０２】また、本実施形態では、力補助装置を医療
顕微鏡２のパワーアシスト用に使用しているので、操作
範囲や負荷重量に対してアーム剛性を低くすることと位
置サーボ系を安定動作させることとが不可欠となる。つ
まり、閉ループゲインを小さくせざるを得ないと共に、
指令追従性能を高くする必要がある。本実施形態の力補
助装置によれば、上述した位置サーボ制御部１６を使用
することにより小さな閉ループゲインのフィードバック
制御器２１に対して指令追従性能を高くすることができ
るので、この力補助装置は特に医療顕微鏡に適したもの
とすることができる。

【０１０３】さらに、フィードフォワード補償器２０で
は指令応答のみを有限整定応答特性にしているので、制
御系内部のノイズ等に影響されることはない。ここで、
制御系モデルのゲインを低くしか設定できない場合で
も、軌跡精度の向上と位置決め時間の短縮を図ることが
できる。

【０１０４】さらに、制御系モデルの第１ポールの補償
と第２ポールの補償とを調整機構及びスイッチにより各
々独立して行うことができる。また、指令応答特性をボ
ード線図のゲイン線図のイメージで任意に設定すること
ができる。そして、位置サーボ制御をソフトウェアサー
ボ化した場合、比較的長いサンプリング時間でも良好な
入出力特性を得ることができる。

【０１０５】また、２自由度位置サーボ系の応答性はイ
ンピーダンス制御により得られた速度指令よりも十分に
大きくなるので、見かけ上は速度指令と速度とが一致す
る。このため、マニピュレータスタンド８の固有振動数
に左右されずに動作の安定性を保ちつつ、インピーダン
ス制御を独立して設定することができる。

【０１０６】しかも、本実施形態での位置サーボ制御系
は各関節部Ｊ１〜Ｊ６ごとに別個独立のものなので、制
御の構成を極めて簡素化することができる。

【０１０７】また、本実施形態では顕微鏡２の移動につ
いて移動モードごとの軌道制御を行っているので、その
前提として高精度の位置制御が必要とされている。上述
したモータ制御部７を使用することによりこの前提は満
たされるので、本実施形態のモータ制御部７は被支持機
器の軌道制御を行う力補助装置に対して特に有効とな
る。

【０１０８】更に、本実施形態の力補助装置の制御装置
によれば、操作者が加える操作力の力・トルク情報にイ
ンピーダンス制御部５によりインピーダンス制御を加え
て各関節部Ｊ１〜Ｊ６の動作を行うモータに対して動作
指令を算出し、各モータを制御することにより複数のア
ームＡ１〜Ａ７の移動及び回転の操作にあたり慣性及び
粘性を主体とした自然な操作感を得ることができる。こ
れにより、顕微鏡２の移動及び回転を自然に近くスムー
ズに行うことができる。

【０１０９】なお、上述の実施形態は本発明の好適な実
施の一例ではあるがこれに限定されるものではなく本発
明の要旨を逸脱しない範囲において種々変形実施可能で
ある。例えば、本実施形態では被支持機器として手術用
の顕微鏡２を用いているが、これに限らず他の機器を用
いても構わない。具体的には、本実施形態の力補助装置
を病院や家庭等での介護作業の補助や、一般産業での重
量物運搬の補助や、産業用ロボットのダイレクトティー
チング作業の補助に適用することができる。また、上記
実施形態におけるアームＡ１〜Ａ７は水平方向の回動及
び垂直方向の回動の双方を行い、それによって被支持機
器としての顕微鏡２があらゆる向きを向くことが出来る
ようになっているが、用途によっては水平方向の回動の
み又は垂直方向の回動のみのアーム構成でもよく、ま
た、上下方向移動や水平方向移動を加えてもよい。

【０１１０】

【発明の効果】以上の説明より明らかなように、請求項
１の力補助装置のモータ制御部は、インピーダンス制御
部と、運動学変換部と、外乱を補償する積分補償型であ
り、かつアーム及び関節部の機械共振点を避ける２自由
度制御系であると共に、前記位置指令により前記モータ
の位置サーボ制御を行う各ジョイントの位置サーボ制御
部とを備えているので、インピーダンス制御部及び運動
学変換部により設定されたインピーダンスとアーム及び
関節部の機械共振点とが干渉して動作を不安定にするこ
とを防止できる。これにより、力補助装置の動作の安定
化を図ることができる。

【０１１１】また、位置サーボ制御部は外乱を補償する
積分補償型であるので、重力や摩擦等の定値外乱による
アーム動作への影響を排除することができる。特に重力
に関しては、重力に対する位置オフセットを生ずること
がない。これにより、力補助装置の動作の安定性をより
向上させることができる。

【０１１２】さらに、位置サーボ制御部を複数の関節部
のそれぞれに別個独立に設けることができるので、位置
サーボ制御の構成を極めて簡素化することができる。

【０１１３】また、請求項２の力補助装置のモータ制御
部では、位置サーボ制御部は、閉ループのゲインをアー
ム及び関節部の機械共振点を避けた値に設定したフィー
ドバック制御器と、閉ループの遅れを補償するフィード
フォワード補償器とを備えているので、位置サーボ制御
部の全体として入力に対して遅れを生ずることなく出力
を行うことができる。このため、位置サーボ制御部は、
運動学変換部からの位置指令に遅れを生ずることなくモ
ータの位置サーボ制御を行うことができる。これによ
り、モータの動作をインピーダンス制御部及び運動学変
換部で設定した通りに行わせることができるので、アー
ム動作の設定はアーム及び関節部の固有振動数に左右さ
れずに安定した動作を維持しながらインピーダンス制御
部及び運動学変換部のみで独立して行うことができる。
このため、モータ制御を容易に高精度に行うことができ
るようになる。

【図面の簡単な説明】

【図１】本発明に係る力補助装置のモータ制御部の位置
サーボ制御部の一実施形態を示し、（Ａ）はフィードフ
ォワード補償器のブロック図、（Ｂ）はフィードバック
制御器のブロック図である。

【図２】力補助装置の制御装置を示す概念図である。

【図３】マニピュレータスタンドを示す側面図である。

【図４】顕微鏡の取付状態を示す顕微鏡の側面図であ
る。

【図５】マニピュレータスタンドを示すスケルトン図で
ある。

【図６】操作ハンドル及び操作スイッチを示す正面図で
ある。

【図７】操作ハンドルの力・トルクセンサへの取付状態
を示す斜視図である。

【図８】ポイントロックモードでの顕微鏡の移動状態を
示す斜視図である。

【図９】平行移動モードでの顕微鏡の移動状態を示す斜
視図である。

【図１０】フォーカス移動モードでの顕微鏡の移動状態
を示す斜視図である。

【図１１】モータの位置サーボ制御によるゲイン特性を
示すボード線図である。

【図１２】モータの位置サーボ制御による指令応答を示
すブロック図である。

【図１３】位置サーボ制御部のフィードフォワード補償
器を示すブロック図である。

【図１４】力補助装置の制御方法を示すフローチャート
である。

【符号の説明】

５インピーダンス制御部７モータ制御部１４運動学変換部１６位置サーボ制御部１８モータ２０フィードフォワード補償器２１フィードバック制御器Ｊ１〜Ｊ６関節部Ａ１〜Ａ７アーム

Claims

【特許請求の範囲】

【請求項１】関節部で回転可能に連結されてモータに
より回転されるアームを操作する操作力に関する力・ト
ルク情報が入力されて、この力・トルク情報を変換する
と共に前記アームからの角度信号をフィードバックして
前記モータの位置サーボ制御を行う位置指令信号を出力
する力補助装置のモータ制御部において、前記力・トル
ク情報のベクトルを速度指令に変換するインピーダンス
制御部と、前記速度指令を逆運動学変換により前記関節
部の位置指令に変換する運動学変換部と、外乱を補償す
る積分補償型であり、かつ前記アーム及び前記関節部の
機械共振点を避ける２自由度制御系であると共に、前記
位置指令により前記モータの位置サーボ制御を行う各ジ
ョイントの位置サーボ制御部とを備えたことを特徴とす
る力補助装置のモータ制御部。
【請求項２】前記位置サーボ制御部は、閉ループのゲ
インを前記アーム及び前記関節部の機械共振点を避けた
値に設定したフィードバック制御器と、前記閉ループの
遅れを補償するフィードフォワード補償器とを備えるこ
とを特徴とする請求項１記載の力補助装置のモータ制御
部。