JP2022169722A - 機械システムの制御方法，機械システム用コントローラ，ロボットマニピュレータ，および非一時的なコンピュータ可読記憶媒体 - Google Patents

Abstract

【課題】ロボットマニピュレータのような機械システムのための改良された制御システムを提供する。【解決手段】機械システムは、複数の駆動ジョイントと複数のコンポーネントを備え、駆動ジョイントまわりまたは駆動ジョイントにおけるトルクまたは力を測定して測定されたトルクまたは力を表す負荷信号を形成するステップと、機械システムの所望の状態を表す動作要求信号を受信するステップと、各駆動ジョイントの目標形状を示す目標信号を形成するために動作要求信号および負荷信号に応じてインピーダンス制御アルゴリズムを実行するステップと、各駆動ジョイントの形状を測定して形状を表す状態信号を形成するステップと、各駆動ジョイントに対して目標信号によって示された駆動ジョイントの目標形状を状態信号によって示された駆動ジョイントに対する測定された形状と比較することによって駆動ジョイントの一組の駆動信号を形成するステップとを含んでいる。【選択図】図４

Description

本発明は、ロボットアームなどの機械の駆動システムの制御に関するものである。

一般的なロボットマニピュレータは、ジョイントによって互いに連結された一連の剛性エレメントを備えている。剛性エレメントは、アームを形成するために直列に連結されてもよい。ジョイントは、剛性エレメントの相対運動をもたらすように駆動されていてもよい。剛性エレメントは、ベースから延び出し、エンドエフェクタで終端されていてもよい。したがって、ジョイントの動作が、エンドエフェクタを所望の位置に位置決めするために使用されていてもよい。各ジョイントは、回転運動または直線運動を提供するものであってもよい。ジョイントは、任意の適切な手段、例えば電気モータまたは油圧アクチュエータによって駆動されていてもよい。

ロボットが動作している時には、エンドエフェクタを所望の位置に移動させる必要がある。例えば、ロボットは、エンドエフェクタを使用して物体をピックアップすることが必要とされていてもよい。したがって、エンドエフェクタを物体のある場所に移動させる必要がある。これを成し遂げるためには、ジョイントの動きを組み合わせる必要がある。これらの動きを計算することが、ロボットの制御システムの役割である。

従来、ロボットには位置センサが設けられており、各位置センサはジョイントのそれぞれの形状を感知している。この位置情報は、制御システムに送り込まれる。

制御システムのためのよく知られた手順は以下の通りである。
１．エンドエフェクタの所望の位置を示す情報を受信する。
２．エンドエフェクタがその位置にくるように、ロボットのジョイントの一組の目標形状を決定する。このことは逆運動学として知られている。
３．ロボットの各ジョイントの現在の形状を示す情報を受け取り、現在の形状を目標形状と比較し、各ジョイントの現在位置と目標位置の誤差を減らすために各ジョイントに必要な一式のトルクまたは力を計算する。
４．計算されたトルクまたは力を各ジョイントに加えるために、ロボットのアクチュエータに駆動信号を送る。

この一連のステップは、時間の経過に伴いロボットの動きが目標形状に一致するように繰り返し実行される。エンドエフェクタの位置を示す代わりに、またはそれに加えて、システムへの情報入力が、ロボットの別の部分の所望の位置を示すものであってもよい。このことは、例えば、ロボットアームが、ロボットアームの中間点付近で外部障害物を避ける必要がある場合に適用され得る。

別のアプローチが、インピーダンス制御として知られている。インピーダンス制御は、（ｉ）トルク／力と、（ｉｉ）位置，速度および加速度との間の関係を調整することを含んでいる。

特定の用途では、エンドエフェクタのインピーダンスを制御する方が位置だけを制御するよりも有利である。例えば、エンドエフェクタが長いロッド（例えば、内視鏡）を小さな開口（例えばポート）を通して挿入する必要がある用途では、ポートと内視鏡との間のわずかな合わせずれを調節することができるため、エンドエフェクタが有限の剛性を有する方が有利である。

図１は、ロボットマニピュレータに対してインピーダンス制御を実行する１つの方法を示している。このアプローチでは、以下の入力がインピーダンス制御ブロックに送り込まれる。
―コマンドソースに由来する、ｐ_d（所望の位置ベクトル），Ｒ_d（所望の回転行列），ｖ_d（所望の速度）及びｖ’_d（所望の加速度）
―順運動学ブロック内の順運動学によって導出される、ｐ_c（対応する座標系における位置ベクトル），Ｒ_c（対応する座標系における回転行列）およびｖ_c（対応する座標系における速度）；および、
―ｈ_c（測定されたエンドエフェクタのトルク／力）。

インピーダンス制御ブロックは、ジョイントスペースで要求される加速度（ａ）を生成するために、これらの入力に基づいてインピーダンス制御モデルを実行する。要求される加速度は、ｑ_dが指示されたジョイント運動ｑである場合にはｄ²ｑ_d／ｄｔ²のベクトルとして表される。その後、これらは逆動力学ブロックによって調整されて、ロボットマニピュレータを駆動するために用いられる一組の駆動トルクまたは力τを発生させる。このアプローチでは、インピーダンスはエンドエフェクタで統合されることが分かる。この場合には、エンドエフェクタにおける接触トルク／力（ｈ_c）を測定しなければならないという欠点があり、このことはいくつかの状況では困難または不可能であろう。例えば、いくつかの外科用ロボットの用途では、追加コストおよび潜在的な非信頼性を伴う。このアプローチのさらなる問題は、逆動力学のモデリングに依存する。それは、マニピュレータの加速度の継続的な計算を必要とし、制御性能をロボットマニピュレータのモデルの精度に敏感にさせ、質量，慣性，損失などのデータが不正確になる可能性がある。さらなる問題は、インピーダンス制御の手法が、駆動摩擦や予想外の物理的障害のような、モデルの範囲外にある外乱に対して敏感なことである。これらは、位置追跡エラーにつながる。

図２は、インピーダンス制御を実行する第２の方法を示している。このアプローチでは、内部動作制御ループが準備される。この方法の目的は、インピーダンス制御ブロックによって計算されたように対応する座標系において要求された位置ｐ_cと回転Ｒ_cを忠実にたどるため、マニピュレータを硬くすることである。指示された値ｐ_d，Ｒ_d，ｖ_dおよびｖ’_dは、測定されたｈ_cと共にインピーダンス制御ブロックに提供される。インピーダンス制御ブロックは、ｖ_dcおよびｖ’_dcに対して数１に示す２次の動力学方程式を解くことによって、エンドエフェクタの所望の機械的インピーダンスを統合する。

この式において、Ｋ_Mはばね定数の行列を表し、ｖ_dcは要求座標系と対応する座標系との間の位置の差を表す。対応する座標系における所望の位置，回転，速度および加速度は、位置および方向制御ブロックに渡される。このブロックは、インピーダンス制御ブロックから決定された値と順運動学によって決定された値との間の差に基づいてシステムの追跡性能を設定するコントローラである。位置および方向制御ブロックの出力は、図１の方式のように処理される、ジョイントスペースにおける要求された加速度を表す一組の出力トルク又は力のセットである。図２の方式は、図１の方式における位置追跡の問題に対処するのに役立つかもしれないが、他にいくつかの欠点がある。第１に、エンドエフェクタのデカルト座標系（ｈ_cは、ｘ，ｙ，ｚ座標系における力，および３つの回転次元におけるトルクを表す）でインピーダンスが計算される。用途によっては、他の座標系を使用することが望ましい場合がある。第２に、図２の方式は、位置および方向の制御に逆運動学の問題をもたらす。通常、逆運動学の問題は解決しにくいと考えられることから、このことは問題となる。この１つの理由は、複数の解決策が存在し得る（すなわち、特定のエンドエフェクタ位置を与えるジョイントの角度のセットが複数存在し得る）ためである。もう一つの理由は、マニピュレータの特定の姿勢が特異となることがあり、有限のジョイント速度ではあらゆる方向にエフェクタを次々に動かすことができないためである。これらの理由から、逆運動学の解をフィードバックループ内にはめ込むことは、全体としてアルゴリズムの潜在的な障害事例を検証することを非常に困難にする。最後に、図１の方式の場合と同様に、エンドエフェクタでトルク／力が測定されるが、このことは非実用的であり、上述のように追加コストおよび非信頼性をもたらす可能性がある。

ロボットマニピュレータのような機械システムのための改良された制御システムが必要とされている。

本発明によれば、機械システムの制御方法は、複数の駆動ジョイントによって相互連結された複数のコンポーネントを備え、この方法は、前記駆動ジョイントまわりまたは前記駆動ジョイントにおける、トルクまたは力を測定して、測定された前記トルクまたは前記力を表す負荷信号を形成するステップと、前記機械システムの所望の状態を表す動作要求信号を受信するステップと、各前記駆動ジョイントの目標形状を示す目標信号を形成するために、前記動作要求信号および前記負荷信号に応じてインピーダンス制御アルゴリズムを実行するステップと、各前記駆動ジョイントの形状を測定して、測定された前記形状を表す状態信号を形成するステップと、各前記駆動ジョイントに対して、前記目標信号によって示された前記駆動ジョイントの前記目標形状を、前記状態信号によって示された前記駆動ジョイントの測定された前記形状と比較することによって、前記駆動ジョイントに対する一組の駆動信号を形成するステップとを含んでいる。

この方法は、各駆動信号に応じて各駆動ジョイントを駆動するステップを含んでいてもよい。各駆動ジョイントは、駆動ジョイントを駆動するためにそれぞれ電気モータを備えていてもよい。各駆動信号は、それぞれの電気モータに印加されていてもよい。

この方法は、第２の測定ステップ，形成ステップおよび駆動ステップを繰り返し実行するステップを含んでいてもよい。一組の駆動信号を形成するステップが、目標信号を形成するステップよりも高い周波数で実行されていてもよい。

インピーダンス制御アルゴリズムが、各駆動ジョイントに対して、それぞれの質量，ダンパーおよびばね項に応じて実行されていてもよい。

動作要求信号は、各駆動ジョイントに対する所望の形状を表していてもよい。

この方法は、機械システムの一部の所望の物理的位置を表す最初の動作要求信号を受信するステップと、機械的システムの一部を所望の物理的位置に位置決めする各駆動ジョイントの形状を決定するための逆運動学的計算を実行するステップと、これらの形状を動作要求信号として提供するステップとを含んでいてもよい。

この方法は、第１の座標空間における機械システムの所望のインピーダンス特性を表すデータを受信するステップと、そのデータを各駆動ジョイントに対してそれぞれの質量，ダンパーおよびばね項に変換するステップとを含んでいてもよい。

第１の座標空間は、デカルト，非デカルト，位相またはベクトル空間であってもよい。

インピーダンス制御アルゴリズムは、ジョイントスペースで実行されてもよい。

動作要求信号は、機械システムの一部の所望の物理的位置を表していてもよい。

インピーダンス制御アルゴリズムを実行するステップは、機械システムの一部の目標物理位置を決定するためにインピーダンス制御アルゴリズムを実行するステップと、機械的システムの一部を目標物理位置に位置決めするのに適した各駆動ジョイントの形状を決定する逆運動学的計算を実行するステップと、駆動ジョイントの目標形状としてその形状を示す目標信号を形成するステップとを含んでいてもよい。

この方法は、機械システムの所望の形状を示す追加情報を特定するステップを含んでいてもよい。逆運動学的計算を実行するステップは、機械的システムの一部を目標物理位置に位置決めすると共に、追加情報によって特定された所望の形状を満足するのに適する、各駆動ジョイントに対する形状を決定するように実行される。

この方法は、駆動ジョイントのまわりまたは駆動ジョイントで測定されたトルクまたは力を、負荷信号を形成するために測定された空間とは異なる第１の座標空間に変換するステップを含んでいてもよい。

第１の座標空間は、デカルト空間，非デカルト空間，位相空間またはベクトル空間であってもよい。

機械システムは、ロボットマニピュレータであってもよい。

機械システムは、外科用ロボットであってもよい。外科用ロボットのエンドエフェクタは、外科用ツールであってもよい。

機械的システムは、マスタースレーブマニピュレータであってもよい。動作要求信号は、マスタコントローラによって形成されていてもよい。

本発明の第２の態様によれば、機械システム用コントローラは、上記の方法を実行するように形成されている。

本発明の第３の態様によれば、ロボットマニピュレータは、複数の駆動ジョイントによって相互連結された複数のコンポーネントと、上記の方法によってロボットマニピュレータを制御するように形成されたコントローラとを有している。

本発明の第４の態様によれば、非一時的なコンピュータ可読記憶媒体には、コンピュータシステムで実行されると、コンピュータシステムに上記の方法を実行させるためのコンピュータ可読命令が格納されている。

機械的システムの一部は、エンドエフェクタまたはエンドエフェクタの近位部分であってもよい。機械的システムの一部の所望の位置は、一部の固有の位置または一部の許容される軌跡を定めることによって定義されていてもよい。

コンポーネントは、剛性を有しおよび／または細長い構成であってもよい。ジョイントは、回転ジョイントおよび／または直線ジョイントであってもよい。

本発明を、添付図面を参照して説明する。

従来技術のインピーダンス制御アルゴリズムを示すブロック図。 別の従来技術のインピーダンス制御アルゴリズムを示すブロック図。 ロボットアームの一例を示す図。 制御ユニットの構成の一例を示す図。 第１のインピーダンス制御アルゴリズム（“Ａ”）を示すブロック図。 第２のインピーダンス制御アルゴリズム（“Ｂ”）を示すブロック図。

図３は、ロボットマニピュレータアームの一例を示す。アームは、ベース１から伸長し、所定の位置に固定されていてもよいし、例えばトロッコ上で可動とされていてもよい。アームは、エンドエフェクタ１１で終端する一組の剛性エレメント２～１０から構成されている。剛性アームエレメントは、再構成可能なジョイント１２～１９によって直列に結合されている。この例では、ジョイントはすべて回転ジョイントであるが、直線運動を提供できるものであってもよい。各ジョイントには、ジョイントの位置形状を検出するためのそれぞれの位置センサ２０と、ジョイント周りのトルクを検出するためのトルクセンサ２１とが備えられている。これらのセンサ２０，２１からのデータは、制御ユニット２２に供給される。ジョイントによって相互接続された２つのアームエレメントのジョイントにおける相対的な回転を駆動するために、各ジョイントには電気モータ２３が備えられている。明確化のために、図３には、センサ２０，２１および電気モータ２３のうちのいくつかのみが示されている。電気モータ２３は、制御ユニット２２からの信号によって駆動される。制御ユニット２２はコンピュータであってもよい。制御ユニット２２は、コマンドデバイス２４から入力を受け取る。その入力は、マニピュレータが特定の方法で動くための刺激または要求を意味する。コマンドデバイスは、例えば、ユーザがアームの所望の動きを信号で伝えることができる、ユーザからの物理的なコマンド入力を検知できてもよい。コマンドデバイスは、１つまたは複数のジョイスティックまたは他の物理的に可動のコントローラを含んでいてもよく、（例えば、ユーザを示すビデオストリームの分析を用いて）ユーザの動きを非接触で感知するためのセンサを備えていてもよく、または格納されているプログラムに従ってアームの動作を自動的に実行するコンピュータであってもよい。そのようなコマンドコンピュータは、制御コンピュータと機能的および／または物理的に一体化されていてもよい。コマンドデバイスは、制御ユニットと同じ位置に配置されていてもよいし、制御ユニットから遠く離れていてもよい。制御ユニットは、ロボットマニピュレータと同じ位置に配置されていてもよいし、ロボットマニピュレータから遠く離れていてもよい。通信リンクは、アームと制御ユニットとの間、および制御ユニットとコマンドデバイスとの間に備えられている。これらは独立して、有線および／または無線リンクとすることができる。このタイプのアームに関するさらなる情報は、国際公開第２０１５／１３２５４９号に開示されている。

制御ユニット２２は通常コンピュータである。単一のハウジング内にあるか、または複数の物理的ユニットの間や異なる場所の間に潜在的に分配されている。図４は、制御ユニット２２の構成の一例を示している。この例では、制御ユニット２２は、プロセッサ４１によって実行可能なプログラムコードを非一時的に格納するメモリ４０を備えている。一時的メモリ４２（例えば、ＲＡＭ）は、プロセッサ４１によって使用可能である。プロセッサ４１は、インターフェース４３および４４に結合されている。インターフェース４３は、アームのセンサおよびモータとインターフェースで接続している。インターフェース４４は、コマンドデバイスとインターフェースで接続している。メモリ４０に格納された非一時的プログラムコードは、プロセッサ４１が所望の制御アルゴリズムを実行するのに必要な機能、例えば、以下にさらに説明するようなアルゴリズムＡおよびＢの１つを実行するために、準備されている。あるいは、制御ユニット２２の機能の一部または全部は、専用ハードウェアで実行されていてもよい。

以下、２つの制御アルゴリズムＡおよびＢについて説明する。各アルゴリズムにおいて、アームのジョイントに対して一組の所望の形状（ｑ_d）が計算される。ｑ_dは、アームの各ジョイントの形状を特定するベクトルであってもよい。形状は、各ジョイントの動きに応じて、角度および／または直線形状であってもよい。所望の形状と、アームの位置センサ２０によって感知される現在の形状とが比較される。各ジョイントの所望の形状とその現在の形状との間の差異に応じて、ジョイントスペース位置コントローラは、ジョイント周りに適用された際にはジョイントを所望の形状に向かって駆動するトルクを計算する。その後各ジョイントは、好ましくは計算されたトルクをジョイントまわりに実質的に適用するように、それぞれの計算されたトルクに従って駆動される。

インピーダンスモデルは、所望のジョイント形状を計算するために用いられる。インピーダンスモデルは、外部入力と、所望のインピーダンスを達成するように特定される設計的制約とを有している。外部入力は、エンドエフェクタまたはアームの任意の部分に対する予めプログラムされた軌道であってもよいし、またはマスタースレーブマニピュレータの場合のように直接的なユーザ入力によって与えられてもよい。

第１の制御アルゴリズムＡについて、図５を参照して説明する。

第１の制御アルゴリズムは、以下の機能ブロックを含んでいる。すなわち、基準位置逆運動学ブロック５０，ジョイントスペースインピーダンスパラメータ計算ブロック５１，常微分方程式（ＯＤＥｓ）を解くことができるジョイントスペースインピーダンス制御ソルバーブロック５２，およびジョイントスペース位置コントローラブロック５３である。図５はまた、マニピュレータおよびその環境をブロック５４として示している。機能ブロック５０～５３は、ハードウェアおよび／またはソフトウェアの任意の適切な組み合わせで実施されていてもよい。

以下の説明において、項ｐは、適切な座標系内の位置を表すが、それは必ずしもデカルト座標系である必要はない。項ｑは、図３のアームのジョイント１２～１９における一組のジョイント角度を示している。１つ以上のジョイントが直線運動を可能にする他の例では、ｑは、ジョイントの直線的な形状を表していてもよい。

大まかに言えば、第１の制御アルゴリズムは以下のように動作する。
―外力によって摂動される前のエンドエフェクタ１１のベース位置を表す基準位置ｐ_refが、基準位置逆運動学ブロック５０に入力される。基準位置逆運動学ブロック５０は、逆運動学を実行して、ｐ_refに対応する一組の基準ジョイント角度ｑ_refを導出する。
―ジョイントスペースインピーダンスパラメータ計算ブロック５１は、マニピュレータの所望のインピーダンス挙動に対する適切な質量，ダンパー及びばね項をそれぞれ表す一組の行列Ｍ_q（ｑ），Ｄ_q（ｑ）及びＫ_q（ｑ）を算出する。
―ｑ_ref，Ｍ_q（ｑ），Ｄ_q（ｑ）及びＫ_q（ｑ）が、アームのジョイントのトルクセンサ２１で検出されたトルクＦと共に、ジョイントスペースインピーダンス制御ソルバーブロック５２に入力される。そのブロックは、一組の要求されたジョイント角度であるｑ_dを形成するために、インピーダンスに基づく制御システムを記述する関係に従って入力を処理する。
―ｑ_dは、位置センサ２０から導出されたジョイントの測定位置を表すｑと共に、ジョイントスペース位置コントローラブロック５３に入力される。ジョイントスペース位置コントローラブロック５３は、モータが作用するそれぞれのジョイントの所望の形状と実際の形状との間の差に応じて、アームのモータ２３に対する一組の駆動出力を形成する。各駆動出力は、それぞれのモータに対する指令トルクを表す。駆動出力はアームのモータ２３に送られ、アームのジョイントに対して対応するトルク／力を印加する。ジョイントスペース位置コントローラは、比例―微分コントローラであってもよいし、または他の線形または非線形制御法（例えば、比例―積分―微分（ＰＩＤ）、逆動力学を伴うＰＩＤ、フィードバック線形化を伴うＰＩＤ、カルマンフィルタのような推測制御法、ＬＱＧ制御、Ｈ－２またはＨ－無限制御法、ファジー論理制御法、モデル予測制御（ＭＰＣ）法、通称動的行列制御法）コントローラであってもよい。

これを数学的に実現する方法の一例を以下に示す。

所望の質量，ダンパーおよびばね項を設定するために、Ｍ_q，Ｄ_q，Ｋ_qの値が選択される。便利なアプローチは、所望のバネ定数Ｋを選択した後、ＭとＤを選択して、選択された周波数でシステムを大いに減衰させることである。

ジョイントスペースインピーダンス制御ソルバーは、ｑに対して数２を解くことができる。

数２を解くために、例えば数３のような標準的な手法を用いてもよい。

一般的なデカルト空間の場合、Ｍ_qとＤ_qは数４のように表される。

例えば、機械システムが冗長シリアルロボットマニピュレータである場合などのいくつかの状況では、Ｍ_q（ｑ）はフルランクマトリックスでなくてもよい。このような場合、いくつかのアプローチが可能である。
－１つのアプローチは、Ｍ_q（ｑ）^-1を計算する必要のないＯＤＥソルバーアルゴリズムを使用することである（例えば、ルンゲクッタ法またはＢｏｇａｃｋｉ－Ｓｈａｍｐｉｎｅ法）。
―別のアプローチは、例えばＴが恒等行列に類似しているが特定の列が除去された状態で、Ｔ^TＭ_q（ｑ）Ｔが可逆であるような行列Ｔを選択することである。そして、ｒがＴｒ＝ｑとなるように定義された状態で、ｑに対してではなくｒに対して解く。このアプローチの結果、ｒはｑに対して縮小された次元となる。このアプローチは、インピーダンスの制約がジョイント角ｑの特定の部分空間にのみ適用されることが望ましい場合、制御されるシステムが冗長マニピュレータである際に有利であり得る。さらに、Ｔが一定ではなく、代わりにｑに対してパラメータ化されていてもよい。この場合、Ｔの零空間におけるｑの部分空間は、直接割り当てられていてもよい。

動作中、Ｍ_q（ｑ），Ｄ_q（ｑ）およびＫ_q（ｑ）は必ずしも一定ではなく、定期的に更新する必要があってもよいことに留意されたい。一方、時間が経つにつれて、Ｍ_x，Ｄ_qおよびＫ_qは一定であるように選択されていてもよい。

この第１の制御アルゴリズムでは、制御すべき範囲がＴ^Tの零空間にあることを保証することによって、またはこれらの範囲に対して適切に剛性のインピーダンスを特定することによって、位置制御が成し遂げられていてもよい。

基準位置がジョイントスペースに与えられている場合には、基準位置逆運動学ブロックは省略されていてもよい。

インピーダンスパラメータは、任意の適切なスペースに与えられていてもよく、ジョイントスペースに変換されていてもよい。それらがジョイントスペースに与えられている場合、ジョイントスペースにジョイントスペースインピーダンスパラメータ計算ブロック５１は省略されていてもよい。

この第１の制御アルゴリズムを説明するために、図３のロボットアームのジョイント１４について考える。アームの冗長性のため、アームの多くの姿勢では、アームの末端部１０を世界的な座標系に固定したまま、ジョイント１４を自由に調節することが可能である。ジョイント１４が回転した際には、アームの次の最も遠位の部分５は軌道に沿って動く。設計者がこの軌道に沿った動作の機械的インピーダンスを特定したいとする。アームの挙動を記述する行列が、最も近位のジョイント１２から順番にアームのジョイントを扱い、ジョイント１４がアームの近位端から３番目のジョイントであるとすると、Ｔは数５のように設定され得る。

これにより、Ｍ_q，Ｄ_qおよびＫ_qは全てスカラーになる。トルクは、スカラーでもあるＴ^Tτ_mとして与えられる。したがって、ＯＤＥは、ジョイント１４に対してｑ_dのコンポーネントを与えるように解かれてもよい。ｑ_dのこのコンポーネントは、基準逆運動学から計算された他のｑ_dのコンポーネントと組み合わされてもよく、得られた完全なジョイント位置ベクトルは、ジョイントスペース位置コントローラブロック５３の中に一組の点として与えられていてもよい。

第２の制御アルゴリズムＢを、図６を参照して説明する。

第２の制御アルゴリズムは、インピーダンス制御ブロック６０，逆運動学ブロック６１，追加入力ブロック６２，ジョイントスペース位置コントローラブロック６３およびトルク変換ブロック６４の機能ブロックを含んでいる。図６はまた、マニピュレータおよびその環境をブロック６５として示している。機能ブロック６０～６４は、ハードウェアおよび／またはソフトウェアの任意の適切な組み合わせで実施されていてもよい。

一般に、第２の制御アルゴリズムは、以下のように動作する。
－外力によって摂動される前のエンドエフェクタ１１のベース位置を表す基準位置ｐ_refが、ｐ_refと同じ座標系に変換されたジョイント上の測定されたトルク／力を表す、行列Ｍ_q（ｑ），Ｄ_q（ｑ）およびＫ_q（ｑ）とＦ_Zと共に、インピーダンス制御ブロック６０に入力される。インピーダンス制御ブロック６０は、エンドエフェクタの所望の位置ｐ_dを形成するためのインピーダンス制御方式を実行する。
－追加入力ブロック６２から追加の位置入力が提供されていてもよい。これは、１つ以上のジョイントがインピーダンス制御の代わりに位置制御下で動作する際に用いられてもよい。
－逆運動学ブロック６１は、逆運動学を実行して、ブロック６１に与えられたｐ_d値の完全なセットに応じた所望のジョイント角ｑ_dのセットを導出する。これらはジョイント角であり、マニピュレータによって採用されると、エンドエフェクタを所望の位置にもってくる。
－ｑ_dは、位置センサ２０から導出されたジョイントの測定位置を表すｑと共にジョイントスペース位置コントローラブロック６３に入力される。ジョイントスペース位置コントローラブロック６３は、モータが作用するそれぞれのジョイントの所望の形状と実際の形状との間の差に応じて、アームのモータ２３に対する一組の駆動出力を形成する。各駆動出力は、それぞれのモータへの指令トルクを表す。駆動出力はアームのモータ２３に送られ、アームのジョイントに対して対応するトルク／力を印加する。ジョイントスペース位置コントローラは、比例―微分コントローラ、または上記の代替アプローチのいずれかであってもよい。
－トルク変換ブロック６４は、アーム上のセンサ２１によって測定されたトルク／力を受け取り、それらをｐ_refおよびｐ_dに使用されるのと同じ座標系の情報を表すベクトルＦ_Zに変換する。これは、方程式Ｆ＝Ｊ（ｑ）^TＦ_Zに従って行ってもよい。ここでＪはヤコビ行列を表す。ＦからＦ_Zを計算することは、必ずしも一意に可能であるとは限らない（すなわち、Ｊ（ｑ）^(1/T)が存在しない可能性がある）ことに留意されたい。その際、有効な候補からＦ_Zを選択するための論理が提供されていてもよい。

第１の制御アルゴリズムＡにおいて、インピーダンスパラメータは、インピーダンス制御アルゴリズムが実施されるのとは異なる座標系で与えられていてもよい。第２の制御アルゴリズムＢでは、ｐ_refとインピーダンス行列が定義される座標系が、値Ｆが測定される座標系とは異なっていてもよい。好都合なことに、ｐ_refとインピーダンス行列は、インピーダンス自体が特定されている座標系で定義されていてもよい。いずれの場合においても、座標系は、周知の座標系（例えば、球面、平面、楕円柱状、極座標、球状、扇形、トロイダル、高調波またはガウス）であってもよいし、ベクトルのパラメータ化またはトポロジー空間によって定義された任意の座標系であってもよい。座標系は、表現する必要があるデータに適した任意の数の次元を有していてもよい。一例では、座標系は、向きが空間内に固定された３つの直線軸上に定義されたデカルト空間であってもよい。別の例では、座標系は、その向きが空間内に固定された軸上に線形値を表さないパラメータによって定義される非デカルト系であってもよい。要求されるインピーダンス特性に対する適切な座標系を選択することによって、インピーダンス定義行列Ｍ_p，Ｄ_p，Ｋ_pは、必要に応じて一定に保たれていてもよい。これにより、アルゴリズムを実行するために必要な計算量を削減できる。例えば、インピーダンスが球の表面上で統合される場合、Ｍ_p，Ｄ_p，Ｋ_pは２×２行列とでき、インピーダンス制御は２自由度の運動を定義し、他の４自由度は直接、例えばブロック６１から制御されていてもよい。

インピーダンス制御ブロック６０は、機械的インピーダンスに対してＯＤＥを解いた後、逆運動学に位置要求を設定することによって、要求された位置ｐ_dを計算するようになっていてもよい。

いくつかの状況では、インピーダンスマトリクスおよびｐ_refが球面座標で表現されていると都合がよい。球面座標を使用しているときは、次のことが言える。
―逆運動学ブロック６１が部分的には球面座標で動作し、部分的には別の便利な座標系（例えばデカルト座標系）で動作することが効率的であるかもしれない。これを実現する実際の方法は、極角と方位角（θ、φ）に対してインピーダンスを特定してもよいが、球の半径（ｐ）と回転式エンドエフェクタの位置を直接特定してもよい。逆運動学ブロック６１は、指定された球面座標から他のシステムに簡単に変換することができる。球面座標からデカルト座標に変換する場合、標準的な関係（ｘ＝ｐｓｉｎθｃｏｓφ，ｙ＝ｐｓｉｎθｓｉｎφ，ｘ＝ｐｃｏｓθ）が使用されていてもよい。
―ブロック６４で実行されるトルク変換は、２段階で行うことができる。まず、ジョイントスペースで表現されたトルクから適切な手段でデカルトトルクに変換できる。図３に示すマニピュレータの例では、トルクセンサを備えた８つのジョイントを有している。それゆえ、Ｊ（ｑ）^Tの擬似逆行列は、ＦからＦ_Zを決定するのに用いられ得る。Ｆ_zx，Ｆ_zyおよびＦ_zzを力が作用するｘ，ｙおよびｚコンポーネントで表すと、球面座標に対してよく知られている単位ベクトルを使用して、球面座標で作用する力が、数６のように表され得る。

―球面座標を使用することは、極角が９０度に向かうにつれて方位角の小さな変化に対する表面に沿った距離がゼロになる傾向があるため、大きな極角値に対しては望ましくない。

アルゴリズムＡおよびＢの両方において、ブロック５３へのｑのフィードバックを有するブロック５３／６３および５４／６５を通るループは、ｑ_dの生成とは非同期に実行されていてもよい。例えば、フィードバックループは、ｑ_dの生成よりも高い周波数で実行されていてもよい。

制御ユニット２２は、第１および第２の制御アルゴリズムＡ，Ｂのいずれか一方または両方を実行できるようになっていてもよい。マニピュレータの動作に応じて第１または第２のアルゴリズムを適用するかどうかを選択するように構成されていてもよい。第１の制御アルゴリズムは、例えば、インピーダンスがデカルトｘ，ｙ，ｚ座標系および回転自由度で容易に表現される場合、必要なインピーダンスの挙動がエンドエフェクタの座標系に密接にマッピングされる際に特に都合がよい。第２の制御アルゴリズムは、例えば、インピーダンスが球の表面のような任意のＮ次元座標系上でエンドエフェクタの動きに対して定義されている場合、必要なインピーダンスの挙動がエンドエフェクタの座標系に密接にマッピングされない際に特に都合がよい。アルゴリズムの選択に影響を及ぼす可能性のある考慮点には、テストの容易性、計算の複雑さ、および提示の明瞭性が含まれる。

アルゴリズムＡおよびＢのそれぞれにおいて、マニピュレータのジョイントにおけるトルク／力センサ２１から受け取ったトルク／力信号Ｆは、それぞれのアルゴリズムの他の部分で使用される前に調整されていてもよい。例えば、これらは、測定された力／トルクからの重力および／または慣性トルク／力の影響を実質的に打ち消すためにアームの現在の姿勢に応じてモデル化された力を減じることによって調整されていてもよいし、ノイズを除去するようにフィルタリングされていてもよいし、測定オフセットを除去するように処理されていてもよい。

上述のアルゴリズムＡおよびＢによるインピーダンス制御は、機械システムが制御下で周囲と相互作用することが必要な駆動関節型機械システムのような制御システムにおいて有用であり得る。インピーダンスは、任意の必要とされる物理インピーダンスの挙動を提供するように定義されていてもよい。例えば、ロボットマニピュレータのエンドエフェクタが、それと相互作用するユーザにとって軽快に見えることが好都合であってもよい。この場合、エンドエフェクタ位置の所望の挙動は、その位置がエンドエフェクタに加えられる外因性の力に線形に比例する。これは、インピーダンス行列を用いて定義されていてもよい。一般に、インピーダンス行列が、任意の機械的インピーダンス（すなわち、トルク／力と位置との間の任意の関係）を定めることを可能にしていてもよい。インピーダンスは、任意の適切な数の次元で、任意の適切なベクトル空間で定義されていてもよい。インピーダンスは、ロボットマニピュレータに沿った任意の位置に対して定義されていてもよい。

アルゴリズムＡおよびＢにおいて、ジョイントスペース位置コントローラブロックが、オプションで逆動力学を実行してもよい。

上述のアルゴリズムＡおよびＢは、実際に加えられたトルク／力における不確実性を補償しようとする計算を必要としないことに留意されたい。代わりに、アルゴリズムは、測定されたジョイント形状および測定されたトルク／力に応じて出力を形成する。これは、いくつかの従来のアプローチよりも単純で正確であり得る。

アルゴリズムＡとＢでは、逆運動学の問題は位置制御ループとは別に保たれている。この潜在的な利点の１つは、ジョイントスペースにおけるアーム位置制御の問題を、逆運動学の問題とは独立して解き、最適化することができることである。もう１つの潜在的な利点は、逆運動学アプローチを独立して発展させられることである。（図３のような）冗長シリアルマニピュレータの場合、内部位置制御ループに影響を与えることなく冗長性を解決することができる。これにより、マニピュレータの安全性と制御精度を向上させることができる。

上述したように、インピーダンスは任意の座標系で定義されていてもよい。次に、１つまたは複数の自由度において、マニピュレータはインピーダンス制御下で動作していてもよい一方、残りの自由度ではマニピュレータは位置制御下で動作していてもよい。

いくつかの先行技術のアプローチとは異なり、アルゴリズムＡおよびＢは、エンドエフェクタに専用のトルク／力センサを必要としないが、専用のセンサが使用されていてもよい。

アルゴリズムＡとＢは、ジョイントが高いギア比で駆動される場合に特に適している。高いギア比は、最高の出力／容積比をもたらすことができることから、多くの場合都合がよい。電気モータによって直接駆動されるジョイントでは、ジョイントによって供給されるトルク／力を、その駆動モータを通る電流の検査から決定することが多くの場合可能である。これは、ほとんどの電気モータが明確な電流―トルク関係を持っているためである。しかしながら、高（例えば１：１００またはそれ以上）のギア比での駆動トレイン摩擦損失により、高比率駆動におけるモータ側トルクの測定からジョイント出力トルクを推定することは困難かもしれない。代わりに、各ジョイントの最終出力ベアリングまたはリンケージにトルクセンサが組み込まれていてもよい。これにより、モータ側の値からジョイントトルクを推定する必要性を回避できる。加えて、各ジョイント内のトルクセンサの存在は、エンドエフェクタにおける力／トルクセンサの必要性を無効にすることができる。

ロボットまたはマニピュレータは、任意の適切な目的のためのものであってもよい。例えば、産業用ロボットまたは外科用ロボットであってもよい。外科用ロボットの場合、エンドエフェクタは、メス、外科用カッター、外科用ピンサーまたは焼灼器のような外科用ツールであってもよい。

ジョイントは、回転式または直線式の電気モータによって駆動されていてもよいし、油圧または空気圧アクチュエータなどの他の手段によって駆動されていてもよい。これらは、同じ制御アルゴリズムによって駆動されている。

上記の説明では、説明を簡単にするために、アルゴリズムＡおよびＢは、機能ブロックに分割されている。実際には、これらのブロックのうちの２つ以上が構造的に組み合わされていてもよい。

上述のアルゴリズムＡおよびＢは、任意の適切な形態のロボットまたはマニピュレータに適用されていてもよい。それらは、図３に示されるタイプのアームに限定されない。この方法は、アーム以外の手段、例えば加工テーブルに構成されたマニピュレータを制御するために用いられてもよい。これらの方法は、クレーンまたは掘削機のアーム、車両サスペンションシステムおよび移動可能な航空機の飛行面エレメントのような、互いに対して動くように駆動することができる相互連結された部品を有する他の機械的装置を制御するために用いられてもよい。

出願人はこれによって、ここに記載の分離した各個別の特徴および２つ以上のそのような特徴の任意の組み合わせを開示しており、そのような特徴または特徴の組み合わせが当業者の共通の一般的な知識に照らして全体として本明細書に基づいて実施されることが可能な程度に開示している。なお、そのような特徴または特徴の組合せが本明細書に開示される任意の問題を解決するかどうかは関係がなく、またかかる具体的記載が特許請求の範囲を限定するものでもない。出願人は、本発明の態様は、このような個々の特徴または特徴の組み合わせから成ってもよいことを示している。以上の説明に鑑みて、種々の改変が本発明の範囲内でなされ得ることは当業者にとって明らかであろう。

Claims (14)

1. 複数の駆動ジョイントによって相互連結された複数のコンポーネントを備える機械システムの制御方法であって、
   第１の座標系において前記駆動ジョイントまわりまたは前記駆動ジョイントにおける、トルクまたは力を測定するステップと、
   前記駆動ジョイントまわりまたは前記駆動ジョイントにおいて測定された前記トルクまたは前記力を、前記第１の座標系とは異なる第２の座標系に変換して負荷信号を形成するステップと、
   前記機械システムの一部の所望の物理的位置を表す動作要求信号を受信するステップと、
   前記動作要求信号および前記負荷信号およびインピーダンスパラメータの入力を有する常微分方程式を解くことを含むインピーダンス制御アルゴリズムを実行し、前記機械システムの前記一部の目標物理位置を決定するステップと、
   前記機械システムの前記一部を前記目標物理位置に位置決めするのに適した各前記駆動ジョイントの形状を決定する逆運動学的計算を実行するステップと、
   各前記駆動ジョイントの前記目標形状として該形状を示す目標信号を形成するステップと、
   各前記駆動ジョイントの前記形状を測定して、測定された前記形状を表す状態信号を形成するステップと、
   各前記駆動ジョイントに対して、前記目標信号によって示された前記駆動ジョイントの前記目標形状を、前記状態信号によって示された前記駆動ジョイントの測定された前記形状と比較することによって、前記駆動ジョイントに対する一組の駆動信号を形成するステップとを含んでいる、機械システムの制御方法。
2. 各前記駆動信号に応じて各前記駆動ジョイントを駆動するステップを含む請求項１に記載の機械システムの制御方法。
3. 各前記駆動ジョイントは、前記駆動ジョイントを駆動するためにそれぞれ電気モータを備えており、各前記駆動信号が各前記電気モータに印加されている請求項２に記載の機械システムの制御方法。
4. 第２の前記測定ステップ，前記形成ステップおよび前記駆動ステップを繰り返し実行するステップを含んでいる請求項２または３に記載の機械システムの制御方法。
5. 前記一組の駆動信号を形成するステップが、前記目標信号を形成するステップよりも高い周波数で実行されている請求項４に記載の機械システムの制御方法。
6. 前記インピーダンス制御アルゴリズムを実行するステップが、各前記駆動ジョイントに対して、それぞれの質量，ダンパーおよびばね項の入力から前記常微分方程式を解くステップを含んでいる請求項１～５の何れか１項に記載の機械システムの制御方法。
7. 前記第１の座標系における前記機械システムの所望のインピーダンス特性を表すデータを受信するステップと、そのデータを各前記駆動ジョイントに対してそれぞれの質量，ダンパーおよびばね項に変換するステップとを含んでいる請求項６に記載の機械システムの制御方法。
8. 前記第１の座標系は、（ｉ）非デカルト座標空間，（ｉｉ）位相空間，または（ｉｉｉ）ベクトル空間である請求項７に記載の機械システムの制御方法。
9. 前記機械システムの所望の形状を示す追加情報を特定するステップと、
   前記機械システムの前記一部を前記目標物理位置に位置決めすると共に、前記追加情報によって特定された前記所望の形状を満足するのに適する、各前記駆動ジョイントに対する形状を決定するように逆運動学的計算を実行するステップとを含んでいる請求項１～８の何れか１項に記載の機械システムの制御方法。
10. 前記第１の座標系は、（ｉ）非デカルト空間，（ｉｉ）位相空間，または（ｉｉｉ）ベクトル空間である請求項１～９の何れか１項に記載の機械システムの制御方法。
11. 前記機械システムが、（ｉ）ロボットマニピュレータおよびまたは（ｉｉ）外科用ロボットである，およびまたは（ｉｉｉ）前記機械システムがマスタースレーブマニピュレータであり、前記動作要求信号がマスタコントローラによって形成されている請求項１～１０の何れか１項に記載の機械システムの制御方法。
12. 請求項１～１１の何れか１項に記載の機械システムの制御方法を実行するように構成されている機械システム用コントローラ。
13. 複数の前記駆動ジョイントによって相互に連結された複数の前記コンポーネントと、請求項１～１１の何れか１項に記載の機械システムの制御方法によってロボットマニピュレータを制御するように構成されたコントローラとを有しているロボットマニピュレータ。
14. コンピュータ可読命令が格納されており、前記コンピュータ可読命令がコンピュータシステムで実行されると、前記コンピュータシステムに請求項１～１１の何れか１項に記載の機械システムの制御方法を実行させる非一時的なコンピュータ可読記憶媒体。

Images