(57)【特許請求の範囲】
【請求項１】
力制御可能なアームを肢体に装着した状態で直接アームを動かし、前記アームの各軸の角度・角速度・角加速度を用いて外力トルクを算出して直接教示し、その際の前記アームの位置情報を記憶装置に記憶し、記憶したデータを再現することで肢体を運動させる肢体駆動装置において、
直接教示時には、前記アームに加えた外力による前記アームの角度・角速度・角加速度を検知し、前記アームの角度・角速度・角加速度とアーム動特性内部モデルとからアーム動特性補償により前記アーム動特性内部モデルへの前記外力トルクを推定してモータへ出力することで、前記アーム動特性が打ち消されるようにし、再生時には、前記角度・角速度・角加速度から機械的インピーダンスモデルをもとに機械的インピーダンスを実現するため必要なトルクを算出し、前記アーム動特性補償で推定した前記外力トルクに併せて前記モータに出力することで、外力に対する動作に硬さを持たせることを特徴とする肢体駆動装置。
【請求項２】
アーム各軸の角度と角速度のみを用いて、アーム動特性を補償するためのトルクを内部モデルから計算するとともに、粘性項と弾性項からなる目標の仮想インピーダンスを実現するためのトルクも計算することを特徴とする請求項１記載の肢体駆動装置。
【請求項３】
アーム各軸の角速度あるいは角加速度を、検出した角度を用いて推定することを特徴とした請求項１あるいは２記載の肢体駆動装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、肢体にアームを装着した状態で直接アームを動かして教示し、その教示動作を再現実行する肢体駆動装置に関する。具体的には、連続受動運動装置と呼ばれる医療機械、リハビリ機械、または筋力訓練機械に関するものである。
【０００２】
【従来の技術】
位置の直接教示が可能な肢体駆動装置は従来図３のようになっている（特願平6-159414）。図３において、３０１は教示者、３０２は肢体または対象物、３０３はアーム、３０４はアーム３０３を駆動するモータ、３０５はモータ３０４の回転角を検出する回転角検出計、３０６は肢体３０２から加わる力を検出する力センサである。
この肢体駆動装置の制御装置の説明図を図４に示す。図４において、４０１はモータを駆動するサーボコントローラ、４０２はアーム３０３の実際の動特性、４０３は教示者や肢体を含めた外部環境の特性、４０４は力制御の規範となる機械的インピーダンスモデル、４０５はモータ３０４の回転角を記憶する記憶装置である。
機械的インピーダンスモデル４０４の説明図を図５に示す。図５において、５０１は外部環境から加わる力、５０２はアーム、５０３はアームの仮想慣性モーメント、５０４はアームの仮想粘性、５０５はアームの仮想弾性である。
【０００３】
直接教示時において、記憶装置４０５の出力θd は一定に保たれている。この出力は目標角度θr としてサーボコントローラ４０１に入力される。サーボコントローラは、回転角検出計３０５から検出した現在の角度θm をもとにサーボ出力トルクτout を出力する。これによりアーム動特性４０２に基づいて実際のモータ角θm が生じる。アーム先端が教示者や肢体などの外部環境に接触している場合には外部環境により力τ'eが発生する。このτ'eはアーム動特性４０２への入力を減ずる。アーム動特性を規定する運動方程式を式１に示す。
【０００４】
【数１】
0003870403.tif 000002
【０００５】
ここで力制御を行なうにあたり、力センサ３０６によりτ'eを検出し、機械的インピーダンスモデル４０４に入力する。ここでは、仮想慣性５０３、仮想粘性５０４、仮想弾性５０５からなる、式２の運動方程式で表される図５のような１質点の動特性モデルをもとに、アーム角度が平衡点から移動すべき変位角δθ＝θ―θd が算出される。
【０００６】
【数２】
0003870403.tif 000003
【０００７】
この変位角を平衡点θd に加算することによって、新たな目標軌道θr を算出し、これをサーボコントローラ４０１に出力することで、外部環境から加わった力に対して目標角度を変更してならうようになる。とくに直接教示時には、インピーダンスパラメータを小さな値に設定すれば、アームを肢体に装着した状態で教示者が肢体に力を加えたとき、肢体およびアームを軽く動かすことができる。ここで肢体を動かしながら、アームの角度θm を記憶装置４０５に記憶する。
【０００８】
再生時においては、記憶装置４０５に記憶してあったアーム角度を平衡角度θd として出力する。このθd は目標角度θr としてサーボコントローラ４０１に入力され、アーム３０３およびアーム３０３に装着された肢体３０２を駆動する。肢体に及ぼす力τ'eは力センサ３０６により検出され、これが図５の機械的インピーダンスモデルに入力されることにより、アーム角度が平衡点から移動すべき変位角δθ＝θ―θdが算出される。この変位角を平衡点θdに加算することによって、外部環境から加わった力に対して、図５に示す動特性に基づいて目標角度を変更してならうようになる。再生時にはインピーダンスパラメータをある程度大きな値にすれば、直接教示時の角度を再現しつつも、柔らかさを持った動きが実現できる。
【０００９】
【発明が解決しようとする課題】
しかし、従来技術においては、
１．力センサを要するため、機構的に複雑になる。
２．直接教示時にアームを持っても力センサで操作力が計測されないため、アームは動かない。このため、直接教示時の操作方法が制約され、教示が自然に行なえない。
という欠点があった。
そこで、本発明は、力センサを必要とせず、アームのどこに力を加えても直接教示が行なえる、力制御可能な肢体駆動装置を提供することを目的とするものである。
【００１０】
【課題を解決するための手段】
上記問題を解決するため、本発明は、力制御可能なアームを肢体に装着した状態で直接アームを動かし、その際のアームの位置情報を記憶装置に記憶し、記憶したデータを再現することで肢体を運動させる、いわゆる直接教示を行なうことのできる肢体駆動装置において、アームの動特性を考慮した内部モデルを設け、アーム各軸の角度・角速度・角加速度を用いて、アーム動特性を補償するためのトルクを内部モデルから計算するとともに、目標とする仮想の機械的インピーダンスを実現するために必要なトルクも計算し、これらのトルクをモータに加えることで、力センサを用いることなく力制御をおこなうことを特徴とするものである。
【００１１】
【発明の実施の形態】
以下、本発明の実施例を図に基づいて説明する。
図１は本発明の請求項１における実施例を示すものである。
図１において、１０１は教示者、１０２は肢体あるいは対象物、１０３はアーム、１０４はアーム１０３を駆動するモータ、１０５はモータ１０４の回転角を検出する回転角検出計である。
本実施例の制御装置の説明図を図２に示す。
図２において、２０１はアーム１０３の実際の動特性、２０２はアーム１０３の動特性を補償する動特性補償器、２０３は力制御の規範となる機械的インピーダンスモデル、２０４はモータ１０４の回転角を記憶する記憶装置である。
【００１２】
上記構成により、アームに加えた外力による微小なアーム角度変化を検知し、このアーム角度変化とアーム動特性内部モデルとからアーム動特性への入力を推定し、これをモータに出力することで、動特性は打ち消され、アームを教示者あるいは肢体からの外力によって軽く動かすことができる。これにより外力を検出する力センサは不要となる。さらに角度変化から機械的インピーダンスモデルをもとにトルクを算出し、これもモータに併せて出力すれば、外力に対する動作にある程度の硬さを持たせることができる。
この制御方法は、外力によってモータ出力軸が動くことが前提になっているので、減速比の低いモータを用いている場合に限られる。アームが肢体や教示者などの外部環境から力τe を受けると、動特性２０１に基づいてモータ角度θm が変化する。この角度は角度検出器１０５により検出される。アーム動特性補償器２０２では、検出したモータ角度θm および角速度・角加速度から、動特性内部モデルに基づいてアーム動特性２０１への入力τa の推定値τb を算出する（式３）。
【００１３】
【数３】
0003870403.tif 000004
【００１４】
またモータ角度θm と平衡点θd との差、およびその変化量は機械的インピーダンスモデル２０３に入力され、ここで慣性・粘性・弾性からなる仮想の１質点系の動特性モデルを実現するための外力トルクτc が算出される（式４、あるいは式５、６を用いてもよい）。
【００１５】
【数４】
0003870403.tif 000005
【００１６】
【数５】
0003870403.tif 000006
【００１７】
【数６】
0003870403.tif 000007
【００１８】
τb とτc との差をτout としてモータに出力することにより、アームの動特性は打ち消され、アームは新たに設定した機械的インピーダンスモデルに基づく動特性で動くようになる。
【００１９】
直接教示時には平衡点θd を固定し、インピーダンスパラメータを小さく設定すれば、アームを肢体に装着した状態で教示者が肢体あるいはアームに力を加えたとき、肢体とアームを軽く動かすことができる。ここで肢体を動かしながら、アームの角度θm を記憶装置２０４に記憶する。再生時においては、記憶装置２０４に記憶してあったアーム角度を平衡角度θd として出力する。このθd と実際の角度θm との間に差が生じると、機械的インピーダンスモデルによりモータトルクが発生する。再生時にはインピーダンスパラメータをある程度大きな値にすれば、  直接教示時の角度を再現しつつ、柔らかさを持った動きが実現できる。
動特性補償器２０２を決定するためには、特開平5-253875にあるように、アームを剛体振子と考え、運動方程式が式７で表されるとする。
【００２０】
【数７】
0003870403.tif 000008
【００２１】
Ｉ, Ｄ, Ｃといった動特性を規定するパラメータを同定するため、モータのトルクを式８のように定める。
【００２２】
【数８】
0003870403.tif 000009
【００２３】
ここでＫは仮想のバネ係数である。すると式９が得られる。
【００２４】
【数９】
0003870403.tif 000010
【００２５】
バネ係数Ｋを適当に与えたときのステップ応答から振幅と周期を計測し、減衰係数ζと振動周波数ｆr およびクーロン摩擦係数Ｃを計算する。このζとｆr から、ＩとＤを求めることができる。
【００２６】
請求項２は、動特性補償器や機械的インピーダンスモデルが角加速度まで考慮していない簡略化したものの場合である。このとき機械的インピーダンスモデルは仮想慣性が無視され、仮想弾性と仮想粘性のみのものとなる。アーム各軸の角速度あるいは角加速度は、タコジェネレータあるいは加速度計などで実測することも可能で、また請求項３に述べるように、ポテンショメータなどの回転角検出計により検出した角度から推測することも可能である。
【００２７】
請求項４は、アームを駆動するモータへの出力を直接教示時にはカットあるいは低減し、再生時には接続あるいはもとの値に戻すような切替え装置を有することを特徴とするものである。これにより、直接教示時には力制御を行なわなくても、サーボが切られているため、モータは教示者の力によって自由に回転し、これによりアームを自由に動かすことができる。再生時以外にはモータに電流を送る必要がないので、電力消費が少なく、また万一の暴走時にもモータは動かないので安全である。
【００２８】
【発明の効果】
以上述べたように、本発明によれば、
１．力センサが不要なので、機構的に簡単になる。
２．直接教示時にアームを持って教示することも可能である。このため、直接教示時の操作方法の制約が少なく、教示が自然に行なえる。
という効果がある。
【図面の簡単な説明】
【図１】本発明の実施例を示す図
【図２】本発明における制御装置の概念ブロック図
【図３】従来の直接教示が可能な肢体駆動装置を示す図
【図４】従来方法における制御装置の概念ブロック図
【図５】機械的インピーダンスモデルの説明図
【符号の説明】
１０１…教示者
１０２…肢体あるいは対象物
１０３…アーム
１０４…モータ
１０５…回転角検出計
２０１…アーム動特性
２０２…動特性補償器
２０３…機械的インピーダンスモデル
２０４…記憶装置
【図１】
0003870403.tif 000011
【図２】
0003870403.tif 000012
【図３】
0003870403.tif 000013
【図４】
0003870403.tif 000014
【図５】
0003870403.tif 000015