JP3377740B2 - 力補助装置の制御方法及びこの方法を利用した制御装置 - Google Patents

Description

【発明の詳細な説明】

【０００１】

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

【０００２】

【従来の技術】力補助装置は、高齢者または一般人であ
っても重さの重い物を操作者の小さい力を増幅すること
によりその物を容易に移動可能となるように支持する装
置である。例えば医療の分野では、医者が手術中に患部
を拡大して見る顕微鏡が大型である場合に、小さな力で
顕微鏡を移動可能に支持するために力補助装置を使用す
ることがある。

【０００３】この場合の力補助装置としては、顕微鏡を
錘やばね等のバランサにより支持して結果的に手動の小
さい力で移動させるものや、バランサで支持しながら電
気又は油圧のモータを動力源にしてジョイスティックや
スイッチの操作により顕微鏡を一定方向に移動または回
転させるもの（例えば、特公平５−３３０４号参照）が
知られている。

【０００４】

【発明が解決しようとする課題】しかしながら、上述し
たバランサにより顕微鏡を移動させる力補助装置では、
顕微鏡の位置及び向きの変化や顕微鏡の交換により重量
バランスが変化して操作者が必要とする操作力が変わっ
てしまうので、操作性が良くない。また、バランサの調
整作業が煩雑であるため、メンテナンス等が面倒となっ
てしまう。さらに、このバランサによるものでは顕微鏡
の焦点位置を固定したままでその焦点位置の見る方向を
変えるように周囲を移動させるいわゆるポイントロック
動作（一定の軌道上で移動または回転可能となる設定）
をすることができない。

【０００５】また、上述したジョイスティックやスイッ
チの操作により顕微鏡を一定方向に移動または回転させ
る力補助装置では、顕微鏡を決められた一定方向にしか
移動または回転できないので、操作性が良くない。さら
に、ジョイスティックやスイッチのオンオフ動作により
顕微鏡を移動させるので、顕微鏡が急発進や急停止等の
ぎこちない動作を行ってしまい、スムーズな操作感を得
ることはできない。

【０００６】そこで、本発明は、重量物等の被支持物の
位置及び向きに拘わらず操作性が一定であり、またポイ
ントロック等の一定軌道上での移動若しくは回転が可能
で、しかもモータを動力源としながらスムーズな移動ま
たは回転を行うことができる力補助装置の制御方法及び
この方法を利用した制御装置を提供することを目的とす
る。

【０００７】

【課題を解決するための手段】かかる目的を達成するた
め、請求項１の力補助装置の制御方法は、回転可能に連
結された複数のアームの先端部に取り付けられた被支持
機器の近傍に配置される操作ハンドルを備え、操作ハン
ドルに加えられる操作力を複数軸方向の信号として出力
する力・トルクセンサで検出し、この検出された複数軸
方向の力・トルク情報をもとに慣性係数及び粘性係数を
有する各軸ごとの動作速度指令ベクトルをインピーダン
ス制御を加えて計算し、動作速度指令ベクトルから姿勢
ベクトル成分を抽出して、被支持機器の座標系を原座標
系に変換する同時変換行列を用いてアームの各関節部の
動作に変換して、被支持機器の軌道を一定球面上で拘束
される関節部の動作指令を軌道制御を加えて計算し、各
関節部の動作指令に基づいて複数のアームを操作ハンド
ルに加えられる操作力方向に移動させるようにしてい
る。

【０００８】また、請求項２の力補助装置の制御装置
は、回転可能に連結された複数のアームと、複数のアー
ムの各々を駆動するモータと、複数のアームの先端部に
取り付けられた被支持機器と、被支持機器の近傍に配置
される操作ハンドルと、操作ハンドルに加えられる操作
力を複数軸方向の信号として出力する力・トルクセンサ
と、力・トルクセンサにより検出された複数軸方向の力
・トルク情報を、慣性係数及び粘性係数を有する各軸ご
との動作速度指令ベクトルに変換するインピーダンス制
御部と、動作速度指令ベクトルから姿勢ベクトル成分を
抽出して、被支持機器の座標系を原座標系に変換する同
時変換行列を用いてアームの各関節部の動作に変換し
て、被支持機器の軌道を一定球面上で拘束される関節部
の動作指令を計算する軌道制御部と、各関節部の動作指
令に基づいて複数のアームを駆動するモータの制御を行
うモータ制御部とを備えるようにしている。

【０００９】ここで、本明細書中では、「力・トルクセ
ンサ」とは加えられた力に対して所定軸、例えばＸ軸，
Ｙ軸，Ｚ軸の軸方向の並進力と各軸回りの回転力とを同
時に測定可能なセンサを意味する。

【００１０】したがって、請求項１及び請求項２の発明
によれば、操作力の力・トルク情報にインピーダンス制
御を加えて各関節部の動作指令を算出するので、複数の
アームの移動及び回転の操作にあたり調整可能な仮想的
な慣性及び粘性を主体とした操作感を得ることができ
る。これにより、マニピュレータの駆動源としてモータ
を使用しても、被支持機器は円滑に移動される。

【００１１】また、操作力の力・トルク情報に軌道制御
を加えて各関節部の動作指令を算出するので、被支持機
器はポイントロック等による一定軌道上での移動や回転
を行うことができる。さらに、各アームの駆動源として
モータを使用しているので、被支持機器の位置や向きに
より負荷荷重が変化しても操作性が劣化することはな
い。

【００１２】さらに、請求項３の力補助装置の制御方法
及び請求項４の力補助装置の制御装置では、力・トルク
センサを６軸の信号を出力するものであるようにしてい
る。したがって、操作ハンドルの操作力についてＸ，
Ｙ，Ｚ軸の軸方向の並進力と各軸回りの回転力とを検出
することができるので、操作ハンドルの全ての動作が検
知される。

【００１３】また、請求項５の力補助装置の制御方法及
び請求項６の力補助装置の制御装置では、被支持機器は
重量物であるようにしている。したがって、支持や移動
が困難な重量物であっても、力補助装置により移動可能
に支持される。

【００１４】

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

【００３８】

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

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

【００４０】

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

【００４１】

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

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

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

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

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

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

【００４７】

【数４】

【００４８】

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

【００４９】

【数６】

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

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

【００５２】

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

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

【００５４】

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

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

【００５６】

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

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

【００５８】

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

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

【００６０】さらに、図２に示すように、軌道制御部６
とマニピュレータスタンド８として例示する各関節用の
モータ部との間にモータ制御部７が介在されている。モ
ータ制御部７は、運動学変換部１４と位置指令変換部１
５と位置サーボ制御部１６とサーボドライバ１７とを有
し、各関節部Ｊ１〜Ｊ６を駆動するためのモータを制御
する。なお、図２中、モータ制御部７はマニピュレータ
８に接続した図として記載してある。

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

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

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

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

【００６５】ところで、本実施形態では位置サーボ制御
部１６を一般的なＰＩＤ制御器から成るものとしフィー
ドフォワード制御及びフィードバック制御によりモータ
駆動の制御を行っているが、これに限らず他の手段によ
りモータ駆動の制御を行っても構わない。

【００６６】以上により構成した力補助装置の制御装置
の作動を図１に示すフローチャートに沿って説明する。

【００６７】操作者が操作ハンドル３に設けられたプッ
シュスイッチＳＬ１，ＳＬ２，ＳＲ１，ＳＲ２を押して
所望の移動モードを選択しながら操作ハンドル３を操作
すると、その操作によって操作ハンドル３に加えられた
操作力はＦＴセンサ４により検出される。ＦＴセンサ４
は、操作ハンドル３に加えられた操作力を受けて、その
操作力の力またはトルクを６軸方向のＦＴセンサ信号と
して出力する（ステップ１０１）。ＦＴセンサ信号はＦ
Ｔセンサアンプ１１及びＦＴセンサ信号処理部１２で処
理されて、ＦＴセンサ４における６軸方向の力・トルク
に関するデジタル信号に変換される（ステップ１０２，
ステップ１０３）。この信号はさらに、操作力座標変換
部１３によりＦＴセンサ４の取り付けと顕微鏡２の関係
を補正されて顕微鏡の向きに合致した方向の力とトルク
の信号に変換される（ステップ１０５）。

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

【００６９】ステップ１０５で操作力座標変換された信
号は、インピーダンス制御部５によりインピーダンス制
御されて、操作者に慣性と粘性とを感じさせる速度指令
に変換される（ステップ１０６）。さらに、軌道制御部
６では、操作者が操作スイッチＳＬ１，ＳＬ２，ＳＲ
１，ＳＲ２により設定した移動モードに応じて所定の軌
道に沿って顕微鏡２が移動するように、速度指令が選択
または変換される（ステップ１０７）。

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

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

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

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

【００７４】

【発明の効果】以上の説明より明らかなように、請求項
１の力補助装置の制御方法並びに請求項２の力補助装置
の制御装置によると、操作力の力・トルク情報に軌道制
御を加えて各関節部の動作指令を算出することにより、
被支持機器は一定球面上で拘束される動作即ちポイント
ロックによる一定軌道上での移動や回転を行うことがで
き、操作性を向上させることができる。さらに、各アー
ムの駆動源としてモータを使用しているので、被支持機
器の位置や向きにより負荷荷重が変化したときの操作性
の劣化を防止することができる。また、操作力の力・ト
ルク情報にインピーダンス制御を加えて各関節部の動作
指令を算出することにより複数のアームの移動及び回転
の操作にあたり慣性及び粘性を主体とした自然な操作感
を得ることができ、被支持機器の移動及び回転をスムー
ズに行うことができる。

【００７５】

【００７６】さらに、請求項３の力補助装置の制御方法
及び請求項４の力補助装置の制御装置では、力・トルク
センサを６軸の信号を出力するものであるようにしてい
るので、操作ハンドルの操作力についてＸ，Ｙ，Ｚ軸の
軸方向の並進力と各軸回りの回転力とを検出することが
でき、操作ハンドルの全ての動作を検知することができ
る。このため、被支持機器を全ての方向に移動及び回転
させることができる。

【００７７】また、請求項５の力補助装置の制御方法及
び請求項６の力補助装置の制御装置では、被支持機器は
重量物であるようにしているので、支持や移動が困難な
重量物を容易に移動可能に支持することができる。

【図面の簡単な説明】

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

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

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

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

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

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

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

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

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

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

【図１１】モータの位置サーボ制御の一実施形態を示す
ブロック図である。

【符号の説明】

１ 力補助装置の制御装置 ２ 顕微鏡（被支持機器） ３ 操作ハンドル ４ 力・トルクセンサ ５ インピーダンス制御部 ６ 軌道制御部 ７ モータ制御部 Ｊ１〜Ｊ６ 関節部 Ａ１〜Ａ７ アーム

フロントページの続き (72)発明者 平栗 正雄 長野県諏訪郡原村10801番地の２ 株式 会社三協精機製作所 諏訪南工場内 (56)参考文献 特開 平６−246652（ＪＰ，Ａ) 特開 平８−234842（ＪＰ，Ａ) 特開 平５−303425（ＪＰ，Ａ) (58)調査した分野(Int.Cl.⁷，ＤＢ名) B25J 13/08 B25J 9/10 B25J 13/02

Claims (6)

(57)【特許請求の範囲】
1. 【請求項１】 回転可能に連結された複数のアームの先
   端部に取り付けられた被支持機器の近傍に配置される操
   作ハンドルを備え、前記操作ハンドルに加えられる操作
   力を複数軸方向の信号として出力する力・トルクセンサ
   で検出し、この検出された複数軸方向の力・トルク情報
   をもとに慣性係数及び粘性係数を有する各軸ごとの動作
   速度指令ベクトルをインピーダンス制御を加えて計算
   し、前記動作速度指令ベクトルから姿勢ベクトル成分を
   抽出して、前記被支持機器の座標系を原座標系に変換す
   る同時変換行列を用いて前記アームの各関節部の動作に
   変換して、前記被支持機器の軌道を一定球面上で拘束さ
   れる前記関節部の動作指令を軌道制御を加えて計算し、
   前記各関節部の動作指令に基づいて前記複数のアームを
   前記操作ハンドルに加えられる操作力方向に移動させる
   ことを特徴とする力補助装置の制御方法。
2. 【請求項２】 回転可能に連結された複数のアームと、
   前記複数のアームの各々を駆動するモータと、前記複数
   のアームの先端部に取り付けられた被支持機器と、前記
   被支持機器の近傍に配置される操作ハンドルと、前記操
   作ハンドルに加えられる操作力を複数軸方向の信号とし
   て出力する力・トルクセンサと、前記力・トルクセンサ
   により検出された複数軸方向の力・トルク情報を、慣性
   係数及び粘性係数を有する各軸ごとの動作速度指令ベク
   トルに変換するインピーダンス制御部と、前記動作速度
   指令ベクトルから姿勢ベクトル成分を抽出して、前記被
   支持機器の座標系を原座標系に変換する同時変換行列を
   用いて前記アームの各関節部の動作に変換して、前記被
   支持機器の軌道を一定球面上で拘束される前記関節部の
   動作指令を計算する軌道制御部と、前記各関節部の動作
   指令に基づいて前記複数のアームを駆動する前記モータ
   の制御を行うモータ制御部とを備えたことを特徴とする
   力補助装置の制御装置。
3. 【請求項３】 前記力・トルクセンサは、６軸の信号を
   出力するものであることを特徴とする請求項１記載の力
   補助装置の制御方法。
4. 【請求項４】 前記力・トルクセンサは、６軸の信号を
   出力するものであることを特徴とする請求項２記載の力
   補助装置の制御装置。
5. 【請求項５】 前記被支持機器は重量物であることを特
   徴とする請求項１または３記載の力補助装置の制御方
   法。
6. 【請求項６】 前記被支持機器は重量物であることを特
   徴とする請求項２または４記載の力補助装置の制御装
   置。