JP2010524714A - 関節を制御する方法及びトルク速度変換装置 - Google Patents

Abstract

【課題】位置制御ハードウエアを用いた従来の位置制御されたシステム上でコンプライアンスオープンループトルク制御を実現する。
【解決手段】ロボットの関節がトルク命令によって制御される。関節は、位置フィードバックループを有する位置制御装置を有する。関節のトルク命令が受け取られ、位置制御装置を使用してトルク命令を実現するために速度フィードフォワード命令が決定される。速度フィードフォワード命令は位置制御装置に送られ、位置フィードバックループは打消される。位置フィードバックループは、関節の実測された位置である位置命令を位置制御装置に送ることによって打消される。位置フィードバックループは、位置フィードバックループのゲインをゼロに設定することによっても打消される。
【選択図】図７

Description

（関連出願の参照）
本出願は、２００７年４月２３日付けの、“A New Approach for Open-Loop Torque Control on Traditional Joint Position-Controlled Robots”と題する米国特許仮出願第６０／９１３，４２８号、及び２００８年４月７日付けの米国特許出願第１２／０９９，０４７号を基礎とする優先権を主張し、これらの仮出願及び出願はその全体がここに引用される。

（技術分野）
本発明は、ロボットのトルク制御に関する。

特定のタスクを遂行するために、ロボットが所与の軌道を辿ることを命じられる場合は多い。直交座標空間において先端部の位置レベルを制御するための逆運動学を適用することによって、個々の関節位置命令は算出される。個々の関節位置を直接命令することによって、モーションが達成され得る。比例積分微分（ＰＩＤ）制御による典型的な位置制御装置は、各関節レベル制御装置に実装され、関節位置命令は、高度ゲイン制御によって達成される。これは、産業用ロボットに使用される一般的な制御システムである。このようなシステムが使用されるのは、所望のタスクを遂行するためには正確かつ迅速な反応が要求され、このような反応は従来の位置制御によって達成されてきたからである。

位置制御によっても制御される人間形ロボットは多い。従来の位置制御は、人間形ロボット用に、特にバランス制御及び歩行制御用に、適用されて成功を収めてきた。この成功は、ゼロモーメントポイント（ＺＭＰ）制御及び操作に対する正確かつ迅速な反応が、人間形ロボットにとっては高い優先度を有していることによる。人間に近い全身モーション及び単純な操作タスクは、従来の位置制御を使用することによって実現されることも多い。

より進化したロボットは、産業用の環境においてだけではなく、人間の日常環境においても仕事をすることを求められる。これらのロボットは、人間を支援し、家庭、オフィス、公共スペース、病院、被災地等で人間と意思疎通するために使用され得る。人間の日常環境においてロボットを制御するための重要な留意点の一つは、（ｉ）ロボットと外部環境との接触、（ｉｉ）ロボットと人間との接触である。ロボットと外部環境との接触の場合は、ロボットの作業空間が非常に狭くかつ複雑であることに起因して予想不可能な接触が生じることがある。ロボットと人間との接触との場合には、人間のモーションそれ自体が予想不可能である。しかしながら、多くの進化ロボットは接触を想定して設計されていない。外部環境は固定されているという推定、又は接触点が限定されている（例えば、両手両足）という推定のもとに、進化ロボットは設計されている。結果として、環境及び人間との安全な接触に準拠したモーションを開発することは、進化ロボット制御にとっての重要課題である。

全身接触及び全身制御は、小型ロボット向けに使用される場合が多い。小型ロボットは比較的軽いので、ロボットと外部環境との間に働く接触力は無視できる。また、多くの場合、小型ロボットは、接触力を考慮することなく運動学が生成した位置命令によって制御される。人間サイズのロボットに関しては、全身モーションがいくつかのロボットに使用されてきた。これらのロボットは、狭い空間内で、腹臥位置又は這う位置から立ち上がることができる。しかしながら、そのモーションは事前に設定されており、ロボットは、位置制御によって命令されたモーションを辿っているにすぎない。コンプライアンス制御が接触モーションに適用される。しかしながら、環境と接触している間、このモーションは、固定された又は安定した環境に制限される。環境又は人間と接触しながらより進化した全身モーションを実現するためには、人間と同じように身体接触を通じて環境を感知することによって複数のタスクをスムースに遂行できるように、ロボットの姿勢を外力に適合させるべきである。

この課題に対する１つの解決は、ロボットに対してトルク制御を使用することである。トルク制御を実現するために、しばしば、トルク制御のためのトルク制御バードウエアが設計される。測定されたトルクフィードバックデータを補償するトルク制御ユニットによってモータが制御され、所望のトルク命令入力が取得される。しかしながら、トルク制御されたシステムは、ロボット一般に見られる位置制御されたシステムとは明らかに異なるハードウエアを使用する。結果として、位置制御されたシステムとともにトルク制御を使用することは困難となり得る。必要とされているのは、位置制御ハードウエアを用いた従来の位置制御されたシステム上でコンプライアンスオープンループトルク制御を実現する方法である。

（発明の要約）
ロボットの関節であって、位置フィードバックループを有する位置制御装置を有する関節を制御する方法によって、前記必要は満たされる。関節のトルク命令が受け取られ、速度フィードフォワード命令が決定され、位置制御装置を使用してトルク命令を実現する。速度フィードフォワード命令が位置制御装置に送られ、位置フィードバックループは打消される。位置フィードバックループは、実測された関節の位置である位置命令を位置制御装置に送ることによって打消される。位置フィードバックループは、位置フィードバックループのゲインをゼロに設定することによっても打消される。

本発明は、限定としてではなく、例示として図示される。図において、同じ番号は、同じ構成要素を示している。
一実施形態に係る、位置制御システムを有するロボットのモーション制御を示す図である。 一実施形態に係る位置制御システムを示すブロック図である。 一実施形態に係る、モータ制御装置とも称する、図２の関節位置制御装置を示すブロック図である。 一実施形態に係る、フィードバックゲインを示す図２のモータ制御装置を示すモータレベルのブロック図である。 一実施形態に係るトルク制御されたシステムを示すブロック図である。 一実施形態に係る、理想位置制御ユニットを有する関節位置制御装置を示すブロック図である。 一実施形態に係る、トルク速度変換装置（ＴＶＴ）を使用する位置制御装置を有するトルク制御されたシステムを示すブロック図である。 一実施形態に係る、フィードバックゲインを示す図７のシステムの一部を示すモータレベルのブロック図である。 一実施形態に係るＴＶＴを示すブロック図である。 一実施形態に係る、電流命令がモータ電流に等しいと仮定した場合の、図４のモータ制御装置のモータレベルのブロック図である。 一実施形態に係る単純化されたＴＶＴを示すブロック図である。 一実施形態に係る、モータ位置制御装置を有するＴＶＴのシステム内の様々な点における値を追加して示した図８のブロック図である。 一実施形態に係る、関節摩擦モデルを示すブロック図である。 一実施形態に係る、トルクセンサによるトルクフィードバックを示すブロック図である。 一実施形態に係る、トルクフィードバック用の実際の関節トルクを算出するための実際のモータ電流の使用を示すブロック図である。 一実施形態に係る、ダイナミクス計算を有する関節トルクフィードバックを示すブロック図である。

本発明の一実施形態を、図を参照しつつ説明する。複数の図にまたがって同様の参照番号が記される場合、それらは同一の又は機能的に類似した構成要素を意味する。さらに、各参照番号の最も左の数字は、その参照番号が初めて使用された図の番号に対応している。

本明細書において、“ある実施形態”又は“一実施形態”と言う場合は、本発明の少なくとも１つの実施形態に、その実施形態に関連して記述される１つの特徴又は構造が含まれていることを意味する。本明細書のあちこちに“一実施形態では”という語が出現しても、必ずしも同一の実施形態を指しているわけではない。

後記する詳細説明のいくつかの部分は、アルゴリズム用語や、コンピュータメモリ内のデータビット操作を示す象徴的な表現による。これらのアルゴリズム的な説明や表現は、情報処理分野の当業者が、自らの業績の要旨を、同分野の他の当業者に最も効率的に伝えるために用いる手段である。アルゴリズムとは、ここでは、そして一般的にも、ある所望の結果に至る複数のステップ（命令）の首尾一貫したシーケンスのことを言う。ステップとは、物理量に対する物理的操作を要求するステップのことである。通常、必ずしも必要条件ではないが、それらの数値は、記憶され、転送され、合成され、比較されかつ操作され得る、電子的、磁気的又は光学的信号の形を取る。これらの信号のことを、ビット、値、要素、シンボル、文字、語又は番号等と呼ぶことが主として用語の共通化の理由から便宜である。さらに、物理量に対する物理的操作を要求するステップの配列のうちいくつかのものは、一般性を失うことなく、モジュール又はコードデバイスと呼ぶことが便宜である。

しかし、このような用語の全ては適当な物理量と関連付けられており、これらの物理量に付された単なる便宜的なラベルに過ぎない。後記において特段の説明がない限り、明細書本文全体を通じて、“処理”、“計算”、“算出”、“決定”又は“表示”等の用語を用いた説明は、（電子的な）物理量としてのデータを、コンピュータシステムのメモリ、レジスタ又は他の情報ストレージのなかで操作しかつ変形するコンピュータシステムや同様の電子的コンピューティングデバイスの動作や処理のことを言う。

本発明のいくつかの側面は、アルゴリズムの形になったプロセスステップや命令を含む。本発明のプロセスステップや命令は、ソフトウエア、ファームウエア又はハードウエアによって実施され、ソフトウエアで実施される場合は、ダウンロードされることが可能であり、多様なオペレーティングシステムが用いる別のプラットフォームから操作されることも可能である。

本発明は、操作を実行する装置にも関する。この装置は、所与の目的を達成する専用装置であってもよいし、コンピュータに記憶されたコンピュータプログラムによって動作する汎用コンピュータであってもよい。このようなコンピュータプログラムは、コンピュータが読取り可能な媒体に記憶され得る。その媒体とは、コンピュータシステムバスに接続可能な、フロッピー（登録商標）ディスク、光ディスク、ＣＤ−ＲＯＭ、電磁光学的ディスク、ＲＯＭ、ＲＡＭ、ＥＰＲＯＭ、ＥＥＰＲＯＭ、電磁的又は光学的カード、ＡＳＩＣ又は電子的命令を記憶し得るあらゆる媒体を含むが、これらに限定されない。さらに、本明細書で言うコンピュータは、単体のプロセッサを含んでもよいし、コンピュータ能力を増加させるために複数のプロセッサを用いた装置であってもよい。

ここでいうアルゴリズムやディスプレイは、もともと特定のコンピュータや他の装置に関係している訳ではない。ここで教示される内容に従って、プログラムとともに多様な一般目的システムが使用され得る。又は、必要とされる方法ステップを実行するためにさらに特化した装置を作成することも便宜である。これらのシステムのそれぞれについてどのような構成が必要となるかは、後記する明細書本文から明らかになる。さらには、本発明は特定のプログラム言語を参照して記載されるものではない。本発明の教示を実装するために、多様なプログラム言語が使用され得る。後記において特定の言語に言及した場合、それは本発明の実施可能性及びベストモードを開示するためである。

さらに、本明細書で使用される言語は、主として意味の取りやすさ及び教示目的から選択されているのであって、本発明の主旨を限定するために選択されているのではない。したがって、本発明の開示は、特許請求の範囲にて記述されている本発明の範囲を例示するものであるが限定するものではない。

図１は、一実施形態に係る、位置制御システムを有するロボットのモーション制御を示す図である。所望の先端部軌道が、破線で示されている。軌道とは、時間の経過とともに変化する先端部の位置である。ある特定のタスクを達成するために、この軌道が所望される。個々の関節位置命令及び速度命令は、逆運動学を適用することにより、所望の軌道に基づいて算出される。個々の関節位置は、直接命令されることも可能である。

図２は、一実施形態に係る位置制御システムを示すブロック図である。位置制御システムは、トルクフィードバックループを有さない。位置制御システムは、関節ごとのモーション制御装置及び関節位置制御装置を含む。モーション制御装置は、タスク及び所望の軌道に基づいて、位置命令及び速度命令を関節ごとに算出し、これらの命令を関節位置制御装置に送る。モーション制御装置は、入力として、実際の関節位置及び実際の速度を受け取る。関節位置制御装置は、伝達関数ＰＣＵ（ｓ）を有する関節位置制御ユニット、伝達関数ＰＪ（ｓ）を有する身体関節及びセンサを含む。関節位置制御ユニットは、位置制御ユニットとも呼ばれる。

関節位置制御ユニットへの入力は、モーション制御装置によって供給される、位置命令ｑ_{ｃｍｄ／ｊｎｔ}及び速度命令ｖ_{ｆｆ／ｊｎｔ}である。位置制御ユニットは、位置命令及び速度命令に基づいて制御されるモータを含む。モータは、モータトルクτ_ｍｏｔを生成する。位置制御ユニットは、典型的には、関節の線形ダイナミクスを補償し、かつ、関節のあらゆる非線形ダイナミクスに対処するように設計されている。図２に示されるように、関節位置制御ユニットＰＣＵ（ｓ）及び身体関節ＰＪ（ｓ）の一連の組合せの周りで、負のフィードバックループが閉じており、位置誤差をゼロにしている。

比例積分微分（ＰＩＤ）制御による位置制御装置は、位置制御ユニットごとに実装される。位置制御システムは、位置フィードバックループ及び速度フィードバックループを有し、これらのループ中で、センサによってデータが測定される。実際の位置が、モータに付設されたエンコーダセンサ又は電位差計等によって測定される。実際の速度が、速度センサによって測定され、又は実際の位置データから算出される。一般的に、モータはギアに接続され、身体関節で大きなトルクを生成する。モータ制御の正確さ及び反応の迅速さを改善するために、速度フィードフォワード命令が位置制御ユニットに適用され、位置ゲイン及び速度ゲインが、関節モータごとに調節される。

伝達関数ＰＪ（ｓ）を有する身体関節のダイナミクスは式（１）によって与えられる。

ここで、Ｉ_ｐｈｙは、身体慣性モーメントであり、Ｆ_ｐｈｙは、関節出力での身体線形

図３は、一実施形態に係る、モータ制御装置とも称する、図２の関節位置制御装置を示すブロック図である。モータ制御装置への入力は、位置命令ｑ_{ｃｍｄ／ｍｏｔ}及び速度命令ｖ_{ｆｆ／ｍｏｔ}である。これらの命令は、より上位のレベルのモーション制御装置によって供給される。モータ制御装置は、伝達関数ＰＬ（ｓ）を有する位置フィードバックループ、伝達関数ＶＬ（ｓ）を有する速度フィードバックループ及び伝達関数ＣＬ（ｓ）を有する電流フィードバックループを含む。

図４は、一実施形態に係る、フィードバックゲインを示す図２のモータ制御装置を示すモータレベルのブロック図である。位置フィードバックループにおいては、実際の位置ｑ_{ａｃｔ／ｍｏｔ}が、位置命令ｑ_{ｃｍｄ／ｍｏｔ}から減算されて、その結果に対して位置ゲインＫＰが乗じられる。位置フィードバックループの後、速度フィードフォワード命令が適用され、位置制御の反応速度を改善する。

電流フィードバックループＣＬにおいては、電流ゲインＫＣは、電流比例ゲインＫ_ｉｐ及び電流積分ゲインＫ_ｉｉとからなる。実際の電流ｉ_ｍｏｔをｉ_ｃｍｄから減算することによって、負の電流フィードバックループは閉じられる。ＡＬは、電機子損失を表し、１／（Ｌ_ｍｓ＋Ｒ_ｍ）として定義される。ここで、Ｒ_ｍはモータ抵抗であり、Ｌ_ｍはモータインダクタンスである。ＫＥは、モータトルクの作動によって生成される逆起電力である。モータトルクは、電流フィードバックループからの出力である実際の電流ｉ_ｍｏｔにトルク定数ＫＴを乗ずることによって算出される。ＩＦは、ロータ慣性Ｊ_ｍ及びロータ摩擦Ｂ_ｍから構成され、１／（Ｊ_ｍｓ＋Ｂ_ｍ）として定義される。モータはこのフレームワークによって制御され、身体関節ＰＪは、高度ゲインフィードバック制御を有する命令に応じて操作される。

図５は、一実施形態に係るトルク制御されたシステムを示すブロック図である。システムは、モーション制御装置及び、モータトルク制御装置とも称する関節トルク制御装置を含む。位置制御されたシステムにおいてと同様に、ロボットは、タスクを遂行するために所望の軌道を辿る特定のモーションに耐えるように設計されている。モーション制御装置は、関節トルク制御装置に送られる個々の関節トルク命令τ_{ｃｍｄ／ｊｎｔ}を算出する。モーションは、先端部位置に基づいて設計される。トルク命令は、所定のモーションに基づきクロックティック（ｃｌｏｃｋ ｔｉｃｋ）ごとに算出される。この算出は、質量、慣性及び重心のようなリンクパラメータを特定することを必要とする。重力を補償する重力トルクが計算される。

モータトルク制御装置は、モータトルク制御ユニットとも称するトルク制御ユニット、センサ、及びギアシステムを有する身体関節からなる。トルク制御ユニットは、モータを含む。図２の関節位置制御されたシステムとは異なり、関節トルク制御されたシステムは、モータトルク制御ユニット内に、正確なトルクフィードバックループ又は電流フィードバックループを有する。このループによって、モータは、モーション制御装置からのトルク命令に基づいて直接操作される。トルク制御されたシステムは、位置制御されたシステムにはないトルク制御ユニットを通常必要とする。したがって、位置制御されたシステムからトルク制御されたシステムに移行するには、ハードウエアの修正が必要になる。さらに、重力トルク、カップリングのような動的効果を補償するには、動的計算のための一貫性のあるフレームワークが必要となる。よって、ロボットにトルク制御を実装するのは困難となり得る。

図６は、一実施形態に係る、理想位置制御ユニットを有する関節位置制御装置を示すブロック図である。図３の関節位置制御装置は修正されて、図６においては、伝達関数ＰＣＵ^＊（ｓ）を有する理想位置制御ユニット、結果として生じる伝達関数ＰＪ^＊（ｓ）を有する理想身体関節及び理想トルクτ_ｐｈｙを有している。理想位置制御ユニット（ＰＣＵ^＊（ｓ））は、伝達係数ＡＳ^＊（ｓ）を有する理想実際のサーボを含む。伝達係数ＰＬ^＊（ｓ）を有する理想位置ループの効果は、実際の位置を位置命令として使用することによって、又は、位置フィードバックループゲインをＫＰ＝０であるように設定することによって打消される。よって、モータへの入力は、速度フィードフォワード命令ｖ_{ｆｆ／ｍｏｔ}のみになる。

関節τ_ｐｈｙでのトルクは、式（３）によって与えられる。

関節での非線形効果を無視し得る場合、項ＡＳ^＊（ｓ）及び項ＰＪ^＊（ｓ）は、線形伝達関数として表すことができる。これらの項は、モータのダイナミクスパラメータを使用するブロック図によって、解析的に定義できる。この場合、理想トルクτ_ｐｈｙ（ｓ）が、所望のトルクτ_ｃｍｄ（ｓ）として算出される。

関節での非線形効果を無視し得ない場合、Ｔ（ｓ）は解析的に計算され得ない。しかしながら、Ｔ（ｓ）は、フィードバック制御システム反応の頻度分析から実験的に特定される。特定された閉鎖ループ伝達関数Ｔ（ｓ）及びＰＪ^＊（ｓ）が所与である場合、理想制御装置は、式（４）のように計算され得る。

ここで、理想トルクτ_ｐｈｙ（ｓ）は、理想動的システムの線形部分と関連付けられたトルクとして導出される。したがって、理想身体関節ＰＪ^＊（ｓ）は、式（５）のように定義される。

ここで、Ｆ_ｐｈｙは関節の摩擦であり、Ｉ_ｐｈｙはギアと関節に取り付けられたリンクとの慣性である。

伝達関数ＡＳ^＊（ｓ）を有する理想位置制御ユニットが識別される場合、関節での理想身体トルクτ_ｐｈｙ（ｓ）が所望の動的トルクτ_ｄｅｓである。所望の動的トルクτ_ｄｅｓ（ｓ）に対応する速度入力ｖ_{ｆｆ／ｍｏｔ}（ｓ）は、式（３）から、式（６）のように決定される。

ここで、ＡＳ^＊（ｓ）は、ＡＳ^＊（ｓ）＝ＶＬ^＊（ｓ）・ＣＬ^＊（ｓ）・Ｍ^＊（ｓ）によって定義される識別された位置制御ユニット伝達関数である。

式（６）は、動的速度入力ｖ_{ｆｆ／ｍｏｔ}（ｓ）と位置制御されたシステムのための所望のトルク命令τ_ｄｅｓ（ｓ）との関係を示している。関節レベルでは、式（６）は、式（７）となる。

ここで、ηはギア比であり、他は以下の通りである。

式（６）及び式（７）は、動的速度入力ｖ_{ｆｆ／ｍｏｔ}（ｓ）と図６に示された位置制御されたシステムのための所望のトルク命令τ_ｃｍｄ（ｓ）との関係を示している。一実施形態においては、位置入力とトルク命令との間の関係は、式（９）の通りである。

図７は、一実施形態に係る、トルク速度変換装置（ＴＶＴ）を使用する位置制御装置を有するトルク制御されたシステムを示すブロック図である。このシステムにおいては、モーション制御装置によって生成されたトルク命令は、ＴＶＴによって変換されて、速度フィードフォワード命令及び位置命令となる。速度フィードフォワード命令及び位置命令は、位置制御装置に送られる。この位置制御装置は、一般的にロボット用に使用される従来の位置制御装置であってもよい。ＴＶＴによって生成された位置命令は、位置フィードバックループの効果を打消す実際の位置である。ＫＰ＝０を命令することによって、位置フィードバックループの効果を打消すことも可能である。

図８は、一実施形態に係る、フィードバックゲインを示す図７のシステムの一部を示すモータレベルのブロック図である。ＴＶＴを使用して、トルク命令は、モータ電流命令ｉ_ｃｍｄに直接送られる。ＴＶＴは、従来の位置制御装置上でオープンループトルク制御を使用することを可能にする。ＴＶＴは、位置制御装置のハードウエアを修正することなく、従来の位置制御装置上でオープンループトルク制御を実現することができるので有利である。モータ制御装置がＴＶＴを創出する。

位置制御装置は、次の入力を取得する。すなわち、１）モータへの速度フィードフォワード入力、及び、２）モータへの位置命令入力又はモータへの位置フィードバックループゲインである。一実施形態においては、ＴＶＴは、次の入力を取得する。すなわち、１）実際の位置をモータに命令することによって位置フィードバックループを打消すための実際の位置、及び、２）実際の速度をモータに命令することによって速度フィードバックループを打消すための実際の速度である。

図７は、トルク制御及び従来の位置制御装置を有するシステムのフレームワークを示す。ＴＶＴは、システムを有効にする関数を供給する。ＴＶＴは、モータの位置フィードバックループの効果を打消す。図６の伝達関数ＰＬ^＊（ｓ）を有する位置ループの効果は、実際の位置を位置制御装置に命令することによって、又は、ＫＰ＝０（図４参照）となるように位置フィードバックループゲインを設定することによって、打消される。また、ＴＶＴは、トルク命令を、モータへの速度フィードフォワード命令に変換する。

図６において示され、式（６）において定義されたＡＳ^＊（ｓ）を識別することは有益である。伝達関数ＰＬ^＊（ｓ）を有する位置ループの効果は、実際の位置である位置を命令することによって、又は、ＫＰ＝０として位置フィードバックループゲインを命令することによって打消される。ＡＳ^＊（ｓ）を導出するためには、速度命令ｖ_{ｆｆ／ｍｏｔ}のみが考慮されればよい。システムは、ブロック図によって、及びモータの動的パラメータによって分析的に定義され得る。このシステムにおいては、関節位置命令ｑ_{ｃｍｄ／ｍｏｔ}は、実際の関節位置ｑ_{ａｃｔ／ｍｏｔ}によって命令され、位置フィードバックループの効果を打消す。

速度ゲインＫＰ及びＫＶを乗ずることによって、電流フィードバックループへの入力ｉ_ｃｍｄは、式（１１）となる。

一方で、モータは、操作中にトルクを生成する。モータトルクは、式（１２）のように決定される。

式（１１）を式（１２）に適用すると、τ_ｐｈｙは式（１３）のように決定される。

τ_ｐｈｙをτ_ｃｍｄに置換すると、ｖ_{ｆｆ／ｍｏｔ}は、式（１４）のように定義される。

関節レベルでは、式（６）及び式（１４）を使用して式（１５）が算出される。

図９は、一実施形態に係るＴＶＴを示すブロック図である。ＴＶＴは、トルク定数の逆数ＫＴ^−１、速度ゲインの逆数ＫＶ^−１及び電流ループの逆モデルＣＬ^−１を含む。関節レベル命令をモータレベル命令に変換するには、式（８）が適用される。

電流フィードバックループＣＬが１に等しいと仮定することによって、式（１５）は単純化できる。なぜならば、閉鎖ループの反応は、位置フィードバックループよりもはるかに早いからである。図６においては、モータへの電流命令ｉ_ｃｍｄは、モータ電流ｉ_ｍｏｔに等しいと仮定され、モデルを単純化している。なぜならば、所定のサーボ周波数までは、電流フィードバックループの効果は無視できるからである。

図１０は、一実施形態に係る、電流命令がモータ電流に等しいと仮定した場合の、図４のモータ制御装置のモータレベルのブロック図である。位置ループＰＬ^＊（ｓ）の効果は、位置命令を実際の位置に設定することによって、又は、ＫＰ＝０を命令することによって、打消される。ＡＳ^＊（ｓ）を導出するには、速度命令ｖ_{ｆｆ／ｍｏｔ}のみが考慮されればよい。このフレームワークにおいては、システムは、ブロック図及びモータの動的パラメータによって分析的に定義され得る。関節位置命令ｑ_{ｃｍｄ／ｍｏｔ}は、実際の関節速度ｑ_{ａｃｔ／ｍｏｔ}によって命令され、速度フィードバックループの効果を打消す。

速度ゲインＫＰ及びＫＶを乗ずることによって、電流フィードバックループへの入力ｉ_ｃｍｄは、式（１８）となる。

一方で、モータは、操作中にトルクを生成する。モータトルクは、式（１９）のように決定される。

式（１８）を式（１９）に適用すると、式（２０）となる。

τ_ｐｈｙをτ_ｃｍｄ及びｑ_{ｃｍｄ／ｍｏｔ}に置換すると、式（２１）となる。

関節レベルでは、式（８）及び式（２１）から、式（２２）が算出される。

図１１は、一実施形態に係る単純化されたＴＶＴを示すブロック図である。ＴＶＴは、トルク定数の逆数ＫＴ^−１及び速度ゲインの逆数ＫＶ^−１を含む。関節レベル命令をモータレベル命令に変換するために、式（８）が適用される。

図１２は、一実施形態に係る、モータ位置制御装置を有するＴＶＴのシステム内の様々な点における値を追加して示した図８のブロック図である。ＴＶＴから位置制御装置への入力は、位置命令及び速度フィードフォワード命令である。位置ループＰＬ^＊（ｓ）の効果は、位置を実際の位置とする命令によって、又は、ＫＰ＝０を命令することによって、打消される。位置命令は、実際の位置によって命令され、実際の位置のフィードバックによって打消される。

したがって、速度フィードバックループに対する位置フィードバックループの効果はゼロである。一方で、ＴＶＴは、式（２４）のようにトルク命令を速度フィードフォワード命令に変換する。

位置フィードバックループの効果は打消されるので、式（２４）の速度フィードフォワード命令のみが、速度フィードバックループに送られる。速度フィードバックループにおいて、実際のモータ速度が、速度フィードフォワード命令から減算され、式（２４）の右辺は式（２５）となる。

式（２５）はＫＶを乗じられて、電流命令ｉ_ｃｍｄとして出力される。

結果として、トルク命令が電流フィードバックループに直接送られ、電流命令が、ＴＶＴを使用して、トルク命令から直接制御される。

一般に、モータはギアに接続され、身体関節で大きなトルクを生成する。しかしながら、モータは、前記したＴＶＴにおいてはモデル化されない摩擦も生成する。ＴＶＴは、トルク命令を速度命令に変換するオープンループ関数として機能するので、モータと関節との間の摩擦は、ＴＶＴの応答に影響を及ぼす。摩擦や剛性のようなハードウエア特性が算出され関節トルク命令τ_{ｃｍｄ／ｊｎｔ}のなかに含まれて、実際のロボット内のＴＶＴの応答を改善する。一実施形態においては、摩擦モデルは、（ｉ）クーロン摩擦モデル、（ｉｉ）粘性摩擦及び（ｉｉｉ）静止摩擦を含む。これらの摩擦が、ＴＶＴへのトルク命令に適用され加えられる。図１３は、一実施形態に係る、関節摩擦モデルを示すブロック図である。

摩擦や剛性のようなハードウエア特性は、関節の出力上にマウントされたトルクセンサによって打消される。トルクセンサからの情報を使用することによって、トルクフィードバック制御は、ＴＶＴへのトルク命令に適用できる。図１４は、一実施形態に係る、トルクセンサによるトルクフィードバックを示すブロック図である。トルクセンサは、オープンループトルク命令を閉じること、及び、ＴＶＴに起因しない摩擦の効果を打消すことを可能にする。

図１５は、一実施形態に係る、トルクフィードバック用の実際の関節トルクを算出するための実際のモータ電流の使用を示すブロック図である。トルクセンサが利用可能ではなく、かつ、関節摩擦が小さくモータの電流に影響を及ぼさない場合に実際のモータ電流が使用される。モータ電流に、トルク定数及びギア比を乗じ、実際のトルクを算出する。

ロボットのモーションを改善するために、トルク命令は、ダイナミクス計算を通じて算出され得る。このダイナミクス計算は、関節空間制御における質量、重心、慣性、コリオリ力及び遠心力、タスク空間制御における先端部位置制御を考慮する。ロボットが動くにつれて、ダイナミクスパラメータは、ロボットの姿勢に応じて再計算され、正しいトルク命令がサーボループ時間ごとに生成される。図１６は、一実施形態に係る、ダイナミクス計算を有する関節トルクフィードバックを示すブロック図である。各関節トルク命令は、ダイナミクス計算素子によって算出され、ＴＶＴを通じて各関節に送られる。関節位置及び関節速度のような実際のデータは、センサによって測定されて、ダイナミクス計算素子に戻される。

本発明は、いくつかの実施形態を参照しつつ相当詳細に記述されているが、当業者には理解されるように、他の実施形態も可能である。

Claims (20)

1. ロボットの関節であって、位置フィードバックループを有する位置制御装置を有する関節を制御する方法であって、
   前記方法は、
   前記関節のトルク命令を受け取り、
   前記位置制御装置を使用して、前記トルク命令を実現する速度フィードフォワード命令を決定し、
   前記速度フィードフォワード命令を前記位置制御装置に送り、
   前記位置フィードバックループを打消すこと、
   を含むことを特徴とする方法。
2. 前記位置フィードバックループを打消すことは、
   実測された前記関節の位置である位置命令を前記位置制御装置に送ること、
   を含むことを特徴とする請求項１に記載の方法。
3. 前記位置フィードバックループを打消すことは、
   前記位置フィードバックループのゲインをゼロに設定すること、
   を含むことを特徴とする請求項１に記載の方法。
4. 前記速度フィードフォワード命令を決定することは、
   前記位置制御装置の観察された周波数応答に基づいて伝達関数を決定すること、
   を含むことを特徴とする請求項１に記載の方法。
5. 前記速度フィードフォワード命令は、
   少なくとも一部が、前記関節の摩擦並びにギア及び前記関節に接続されたリンクの慣性に基づいていること、
   を特徴とする請求項１に記載の方法。
6. 前記速度フィードフォワード命令は、
   

   

   に等しく、
   τ_{ｃｍｄ／ｊｎｔ}は、前記関節ごとの前記トルク命令であり、
   ＫＶは、前記位置制御装置の速度ゲインであり、
   ＫＴは、前記位置制御装置のトルク定数であり、
   ＣＬは、前記位置制御装置の電流フィードバックループのゲインであり、
   

   

   ηは、前記関節のギア比であること、
   を特徴とする請求項１に記載の方法。
7. 前記速度フィードフォワード命令は、
   

   

   に等しく、
   τ_{ｃｍｄ／ｊｎｔ}は、前記関節ごとの前記トルク命令であり、
   ＫＶは、前記位置制御装置の速度ゲインであり、
   ＫＴは、前記位置制御装置のトルク定数であり、
   

   

   ηは、前記関節のギア比であること、
   を特徴とする請求項１に記載の方法。
8. 前記関節は、
   オープンループトルク制御を使用して制御されること、
   を特徴とする請求項１に記載の方法。
9. 前記方法は、
   前記関節の摩擦を説明する摩擦モデルに基づいて前記トルク命令を修正すること、
   を含むことを特徴とする請求項１に記載の方法。
10. 前記方法は、
    前記関節の測定されたトルクを受け取り、
    前記関節の摩擦に対応する前記測定されたトルクに基づいて前記トルク命令を修正すること、
    を特徴とする請求項１に記載の方法。
11. ロボットの関節であって、位置フィードバックループを有する位置制御装置を有する関節を制御するトルク速度変換装置であって、
    前記トルク速度変換装置は、
    前記関節のトルク命令を受け取るモジュールと、
    前記位置制御装置を使用して、前記トルク命令を実現する速度フィードフォワード命令を決定するモジュールと、
    前記速度フィードフォワード命令を前記位置制御装置に送るモジュールと、
    前記位置フィードバックループを打消すモジュールと、
    を有すること、
    を特徴とするトルク速度変換装置。
12. 前記位置フィードバックループを打消すことは、
    実測された前記関節の位置である位置命令を前記位置制御装置に送ること、
    を含むことを特徴とする請求項１１に記載のトルク速度変換装置。
13. 前記位置フィードバックループを打消すことは、
    前記位置フィードバックループのゲインをゼロに設定すること、
    を含むことを特徴とする請求項１１に記載のトルク速度変換装置。
14. 前記速度フィードフォワード命令を決定することは、
    前記位置制御装置の観察された周波数応答に基づいて伝達関数を決定すること、
    を含むことを特徴とする請求項１１に記載のトルク速度変換装置。
15. 前記速度フィードフォワード命令は、
    少なくとも一部が、前記関節の摩擦並びにギア及び前記関節に接続されたリンクの慣性に基づいていること、
    を特徴とする請求項１１に記載のトルク速度変換装置。
16. 前記速度フィードフォワード命令は、
    

    

    に等しく、
    τ_{ｃｍｄ／ｊｎｔ}は、前記関節ごとの前記トルク命令であり、
    ＫＶは、前記位置制御装置の速度ゲインであり、
    ＫＴは、前記位置制御装置のトルク定数であり、
    ＣＬは、前記位置制御装置の電流フィードバックループのゲインであり、
    

    

    ηは、前記関節のギア比であること、
    を特徴とする請求項１１に記載のトルク速度変換装置。
17. 前記速度フィードフォワード命令は、
    

    

    に等しく、
    τ_{ｃｍｄ／ｊｎｔ}は、前記関節ごとの前記トルク命令であり、
    ＫＶは、前記位置制御装置の速度ゲインであり、
    ＫＴは、前記位置制御装置のトルク定数であり、
    

    

    ηは、前記関節のギア比であること、
    を特徴とする請求項１１に記載のトルク速度変換装置。
18. 前記関節は、
    オープンループトルク制御を使用して制御されること、
    を特徴とする請求項１１に記載のトルク速度変換装置。
19. 前記トルク速度変換装置は、
    前記関節の摩擦を説明する摩擦モデルに基づいて前記トルク命令を修正するモジュールを有すること、
    を特徴とする請求項１１に記載のトルク速度変換装置。
20. 前記トルク速度変換装置は、
    前記関節の測定されたトルクを受け取るモジュールと、
    前記関節の摩擦に対応する前記測定されたトルクに基づいて前記トルク命令を修正するモジュールと、
    を有すること、
    を特徴とする請求項１１に記載のトルク速度変換装置。

Images