JP7293494B2 - 機械システムにおけるジョイント制御 - Google Patents

Description

本発明は、ロボットアームなどの機械システムのジョイントの構成の制御、特に、トルクセンサーデータを入力として使用せずに、この構成を制御するコントローラーに関する。

典型的なロボットマニピュレーターは、アームを形成するために直列に結合され得る、一つまたは複数のジョイントによって一緒に結合される一連の剛直な要素を含む。マニピュレーターは、一連の剛直な要素の第一の端部にベースユニットユニットと、第一の端部に対向する第二の端部にエンドエフェクターとを含む。マニピュレーターの各ジョイントは、剛直な要素の相対運動を引き起こすように、駆動源および対応するドライブトレインによって駆動される。この相対運動は、所望の位置でエンドエフェクターの構成を変えるために使用される。各ジョイントは、回転運動または直線運動を提供し得る。駆動源は、電子モーターまたは油圧アクチュエーターなどの任意の適切な手段であり得る。

理論的モデリングでは、ロボットジョイントを作動させるドライブトレインを「剛性」としてモデル化することができる。剛性のドライブトレインとは、静摩擦やバックラッシュのないものである。バックラッシュとは、方向の変化により生じる動きの損失を指し、ドライブトレイン内の対向する構成要素間のギャップまたはクリアランスによって生じる。ドライブトレインに静摩擦またはバックラッシュがない場合、駆動源によって生成されるトルク（すなわち、ドライブトレインの第一の端部に提供される入力トルク）は常に、ジョイントによって経験されるトルク（すなわち、第一の端部に対向するドライブトレインの第二の端部における出力トルク）に比例する。

現実には、ロボットジョイントを作動させるドライブトレインの構成要素は、関連するレベルの弾性を有する。従って、ジョイントの剛性は、入力トルクと出力トルクの間の線形関係を用いて正確にはモデル化することができない。従って、複数のフィードバック項、ドライブトレインの入力および出力の両方で測定されるセンサーデータに対応するそれらのフィードバック項を組み入れるアルゴリズムを使用して、駆動源を制御する必要がある。従って、センサーデータを使用して、ジョイントの性能の表示を提供することができる。制御アルゴリズムに複数のフィードバック項を組み込むと、そのアルゴリズムに関連付けられるコントローラーの複雑さが増し、安定性の限界も生じる。

ロボットアームなどの機械システムにおけるジョイントの構成を制御するより単純な方法が必要である。

第一の態様によれば、手術用ロボット内のジョイントの構成を制御するためのコントローラーが提供され、ジョイントが、駆動源からジョイントに電力を伝達するドライブトレインによって駆動され、コントローラーが、駆動源の構成を示す第一の入力を受信し、第一のセンサーから第二の入力を受信することであって、第二の入力が手術用ロボット内のジョイントの測定される構成を示すように受信し、第一の入力および第二の入力を使用して、ジョイント周りの出力トルクの値を計算し、出力トルクの値を使用して、駆動源によって手術用ロボット内のジョイントに印加される入力トルクの値を計算するように構成される。

第一の入力は、第二のセンサーから受信されてもよく、駆動源の測定される構成を示し得る。

駆動源の測定される構成は、駆動源の測定される物理的位置であってもよく、ジョイントの測定される構成は、ジョイントの測定される物理的位置であり得る。

コントローラーは、ジョイントの所望の構成を示す第三の入力を受信するようにさらに構成され得る。

所望の構成は、ジョイントの所望の物理的位置であり得る。

入力トルクの値は、第一の入力と第三の入力との間の比較を使用して計算され得る。

入力トルクの値は、第二の入力と第三の入力との間の比較を使用して計算され得る。

入力トルクの値は、第一の入力の一次導関数と第三の入力の一次導関数との間の比較を使用して計算され得る。

入力トルクの値は、第二の入力の一次導関数と第三の入力の一次導関数との比較を使用して計算され得る。

出力トルクは、ドライブトレインの伸長値とドライブトレインの剛性値との間の関係によって特徴付けられ得る。

出力トルクの値は、方程式τ_ｏ＝ｋφ（ｑ_ｉ－ｑ_ｏ）によって表されてもよく、ｑ_ｉが第一の入力であり、ｑ_ｏが第二の入力であり、ｋφがドライブトレインのばね係数である。

ｋφは連続関数によって（ｑ_ｉ－ｑ_ｏ）に関連し得る。

ｋφが、離散範囲の値から選択されてもよく、ｋφの各値が、一つまたは複数の所定の閾値によって画定される伸長値の範囲と関連付けられる。

ばね係数は、ドライブトレインの測定される伸長に依存する三つの別個の値から選択されてもよく、ここで、測定される伸長の値が第一の所定の閾値を下回る場合、第一の値がばね係数に対して選択され、測定される伸長の値が第一の所定の閾値より高く、かつ第二の所定の閾値より下にある場合、第二の値がばね係数に対して選択され、測定される伸長の値が第二の所定の閾値を上回る場合、第三の値がばね係数に対して選択される。

第一の所定の閾値より上かつ第二の所定の閾値より下である伸長値の範囲は、ジョイントのバックラッシュ領域に対応し得る。

入力トルクの値は、以下の式によって表され得る：

式中、ｑ_ｉは第一の入力であり、ｑ_ｏは第二の入力であり、ｑ_ｒは第三の入力であり、ｋ_ｐｏ、ｋ_ｐｉ、ｋ_ｄｏ、ｋ_ｄｉおよびｋ_ｔは第一、第二、および第三の入力に関連付けられるゲインである。

コントローラーは、出力トルクの値を繰り返し計算するように構成され得る。

コントローラーは、動的トルク観測器内に実装されてもよく、動的トルク観測器は、コントローラーによって計算された出力トルクの値に重み付けを適用することによって、動的トルクの値を計算するように構成される。

入力トルクの値を計算することは、出力トルクの値から重力の作用を補償するトルク項を減算することを含み得る。

駆動源はモーターであり得る。

第一のセンサーは、ジョイントが位置するドライブトレインの第二の端部に、またはその近位の位置に位置し得る。

第二のセンサーは、駆動源が位置するドライブトレインの第一の端部に、またはその近位の位置に位置し得る。

ドライブトレインは、一つまたは複数のギアを含んでもよい。

ドライブトレインは、高調波駆動であり得る。

第二の態様によれば、手術用ロボットにおけるジョイントの構成を制御するための方法（例えば、コンピューター実行方法）が提供され、ジョイントが、駆動源からジョイントに電力を伝達するドライブトレインによって駆動され、方法が、駆動源の構成を示す第一の入力を受信することと、第一のセンサーから第二の入力を受信することであって、第二の入力が手術用ロボット内のジョイントの測定される構成を示すように受信することと、第一の入力および第二の入力を使用して、ジョイント周りの出力トルクの値を計算することと、出力トルクの値を使用して、駆動源によって手術用ロボット内のジョイントに印加される入力トルクの値を計算することとを含む。

ここで、添付図面を参照して、本発明を例として説明する。図は、以下の通りである。

図１は、ロボットアームの構成を示す。 図２は、ロボットアーム内のジョイントの構成を制御するための理想的な制御ループを示すブロック図である。 図３は、ロボットジョイントに対するドライブトレインの非線形剛性の特徴付けを示す。 図４は、ロボットアーム内のジョイントの構成を制御するための全状態制御スキームを示すブロック図である。 図５は、図４に示すコントローラーに対する制御アルゴリズムを示すブロック図である。制御アルゴリズムは、トルクセンサーデータを入力として使用する。 図６は、トルクセンサーデータを入力として使用しないロボットアーム内のジョイントの構成を制御するための制御アルゴリズムを示すブロック図である。 図７は、図６の制御アルゴリズムを使用してロボットジョイントを制御する方法を示す。 図８は、図６の制御アルゴリズムと組み合わせて使用するためのロボットアームの動的トルク観測器を示すブロック図である。

以下の説明は、ロボットシステムの文脈における本技術を説明する。ロボットシステムには、車両製造システム、部品取り扱いシステム、実験室システムなどの製造システム、および危険物などのためのマニピュレーターまたは外科用マニピュレーターが含まれ得る。図１に図示したロボットシステムは、特に手術用ロボットシステムである。しかしながら、以下に記載される特徴は、こうしたシステムに限定されなく、より広くはロボットシステムに適用される。

図１は、ベースユニット１０２から延在するアーム１００を有する手術用ロボットを例示する。アームは、複数の外旋ジョイント１０６ａ～ｅによって結合される複数の剛直な肢１０４ａ～ｅを含む。外旋ジョイント１０６ａ～ｅは、回転運動を適用するように構成される。これらの外旋ジョイントの代わりに、またはこれらに加えて、手術用ロボットは、直線運動を提供するための一つまたは複数のジョイントを含んでもよい。ベース１０２に最も近い肢は、最も近位の肢１０４ａであり、近位ジョイント１０６ａによってベースに結合される。アームの残りの肢はそれぞれ、複数のジョイント１０６ｂ～ｅのジョイントによって直列に結合される。手首１０８は、四つの個々の外旋ジョイントを含んでもよい。手首１０８は、一つの肢（１０４ｄ）をアームの最遠位肢（１０４ｅ）に結合する。最遠位肢１０４ｅは、手術器具１１２のための取り付け具１１０を担持する。アーム１００の各ジョイント１０６ａ～ｅは、それぞれのジョイントで回転運動を引き起こすように操作できる一つまたは複数の駆動源１１４を持つ。各駆動源１１４は、駆動源からジョイントに電力を伝達するドライブトレインによって、それぞれのジョイント１０６ａ～ｅに接続される。一実施例では、ドライブトレインは、一つまたは複数のギアを含む。より具体的な例では、ドライブトレインは高調波駆動である。高調波駆動は、波動発生装置、フレックススプラインおよび円形スプラインの同心配置を含み、駆動源によってジョイントに印加され得るトルクを増加させる。

一実施例では、駆動源１１４はモーターである。駆動源１１４は、代替的に、油圧アクチュエーター、または任意の他の適切な手段であり得る。各ジョイント１０６ａ～ｅは、そのジョイントにおける現在の構成および／または力に関する情報を提供する、一つまたは複数の構成および／または力センサー１１６をさらに含む。ジョイントが外旋ジョイントである場合、センサー１１６のうちの一つまたは複数はトルクセンサーであり得る。ジョイントが直線運動を提供するように構成される場合、一つまたは複数のセンサー１１６のうちの一つは、歪ゲージであり得る。構成および／または力データに加えて、一つまたは複数のセンサー１１６は、温度、電流、または圧力（油圧など）に関する情報を追加的に提供し得る。一実施例では、一つまたは複数の構成センサーは、位置決めセンサーである。すなわち、構成センサーは、ジョイントの物理的位置を測定し得る。こうした実施例では、センサー１１６のうちの一つまたは複数は、エンコーダーであり得る。エンコーダーは、位置測定値を電子信号に変換する位置決めセンサーである。エンコーダー１１６は、線形または回転式エンコーダーであり得る。

駆動源１１４は、重量分布を改善するために、それらが駆動させる運動をするジョイントの近位に配置される。明確にするために、図１は一部の駆動源とセンサーのみを示している。手術用ロボット１００は、図１に図示したものよりも多いまたは少ない肢およびジョイントを含んでもよいことも理解されよう。

手術用ロボット１００の器具１１２は、手術を実行するためのエンドエフェクターを含む。エンドエフェクターは、任意の好適な形態をとり得る。例えば、エンドエフェクターは、滑らかな顎、鋸歯状の顎、グリッパー、一対の剪断器、縫合用針、カメラ、レーザ、ナイフ、ステープラ、焼灼器、吸引器であり得る。器具は、器具シャフトと、器具シャフトとエンドエフェクターの間に位置する関節とをさらに含む。関節は、いくつかのジョイントを含み、これにより、エンドエフェクターが器具のシャフトに対して移動することを可能にする。

駆動源１１４およびセンサー１１６の構成コントローラーは、ロボットアーム１００内に分布する。コントローラーは、通信バスを介して制御ユニット１１８に接続される。制御ユニット１１８は、プロセッサー１２０およびメモリー１２２を含む。メモリー１２２は、非一時的な方法で、プロセッサー１２０によって実行可能であり、駆動源１１４の動作を制御してアーム１００を動作させるソフトウェアを格納する。特に、ソフトウェアは、プロセッサー１２０を制御して、センサー１１６および外科医コマンドインターフェイス１２４からの入力に依存して、駆動源（例えば、分散型コントローラーを介して）を駆動させることができる。制御ユニット１１８は、ソフトウェアの実行によって生成された出力に従ってそれらを駆動するために駆動源１１４に結合される。制御ユニット１１８は、センサーから感知された入力を受信するためにセンサー１１６に電気的に接続され、センサーから入力を受信するためにコマンドインターフェイス１２４に電気的に接続される。図１およびその後の全ての図を参照して記載される電気的接続は、例えば、それぞれ電気または光学ケーブルであってもよく、または無線接続によって提供され得る。

コマンドインターフェイス１２４は、一つまたは複数の入力装置を含み、それによって、ユーザーは、所望の方式でエンドエフェクターの運動を要求することができる。入力装置、または入力コントローラーは、例えば、制御ハンドルやジョイスティックなどの手動操作可能な機械的入力装置、タッチスクリーンなどのタッチ操作可能な入力、または光学ジェスチャーセンサーや音声センサーなどの非接触入力装置であり得る。入力装置は、入力を受信するために目の動きを監視し得る。入力装置は、例えば、これらのタイプの入力装置のいくつかの組み合わせであり得る。入力装置によって入力されるコマンドには、例えば、横方向の移動および／または回転などの特定の方法で器具を動かすための移動コマンドが含まれ得る。こうしたコマンドには、例えば、エンドエフェクターを器具１１２の遠位端に結合して制御し、グリッパー顎を開閉する、または電気外科手術用エンドエフェクターを操作する（オンまたはオフにする）など、エンドエフェクターを動作させるための、エンドエフェクターコマンドが含まれ得る。

メモリー１２２に記憶されるソフトウェアは、これらの入力に応答し、それに応じて、アームおよび器具のジョイントを、所定の制御戦略に従って、移動させるように構成される。制御戦略には、コマンド入力に応答して、アームおよび器具の運動を緩和する安全機能が含まれ得る。従って、要約すると、コマンドインターフェイス１２４を適切に含む外科医コンソールの外科医は、所望の外科的処置を実行するように移動するように器具１１２を制御することができる。従って、ロボットアーム１００は、制御ユニット１１８がマスターコントローラーとして機能する、マスタースレーブマニピュレーターとして機能する。制御ユニット１１８および／またはコマンドインターフェイス１２４は、アーム１００から遠隔にあり得る。

図２は、図１に示すように、ロボットアーム内の外旋ジョイントの構成を制御するための理想的な制御ループ２００を示す。制御ループは、構成コントローラー２０２、トルクコントローラー２０４、慣性出力部２０６およびセンサー２０８を含む。

コントローラー２０２は、制御するように構成されるジョイントの近位にあるように、図１に図示したアームなどのロボットアーム内に分配され得る。構成コントローラー２０２は、フィードバックループによって一つまたは複数のセンサー２０８に電気的に接続される。フィードバックループは、コントローラー２０２が、センサーからジョイントの構成を示す測定値を受信することを可能にする。構成コントローラー２０２はまた、フィードフォワードループによって制御ユニット１１８に接続され、制御ユニット１１８から基準構成値ｑ_ｒｅｆを受信するように構成される。一実施例では、構成コントローラー２０２は、制御ユニット１１８とは別個の実体である。別の実施例では、構成コントローラー２０２および制御ユニット１１８は、同じ実体内に含まれる。従って、この実施例では、制御ユニットは、センサー２０８に直接接続され得る。

基準構成ｑ_ｒｅｆは、そうでなければ、ジョイントの所望の構成または指令された構成と呼んでもよい。基準構成ｑ_ｒｅｆは、外科医によってコマンドインターフェイス１２４で指令されるエンドエフェクター姿勢から計算される。基準構成ｑ_ｒｅｆは、指令されたエンドエフェクター姿勢に一つまたは複数の逆運動学的計算を適用することによって計算され得る。一実施例では、基準構成ｑ_ｒｅｆは、ジョイントの所望の物理的位置である。

構成コントローラー２０２は、制御ユニット１１８から受信した基準位置値ｑ_ｒｅｆおよびセンサー２０８からのフィードバック値を使用して計算を行って、基準トルク値τ_ｒｅｆを生成するように構成される。基準トルク値τ_ｒｅｆは、ジョイントを基準構成ｑ_ｒｅｆに移動するために必要な要求トルクを示す。次に、基準トルク値τ_ｒｅｆがトルクコントローラー２０４に送信される。

トルクコントローラー２０４は、構成コントローラー２０２に電気的に接続され、基準トルク値τ_ｒｅｆを受信するように構成される。τ_ｒｅｆの値から、ジョイントトルクコントローラーは、駆動源によって生成される所望の電流の値を計算するように構成される。この所望の電流は、駆動源によって駆動されるときにジョイントによって経験される実際のトルクである出力トルクτ_ｏを生成するために使用される。理想的なシステムでは、駆動源からジョイントにトルクを伝達するドライブトレインは「剛性」であり、トルクは駆動源からジョイントに伝達される際に「失われ」ない。従って、ジョイントで経験される出力トルクτ_ｏは、構成コントローラー２０２によって受信される基準トルクτ_ｒｅｆと等しい。

剛性でないジョイントは、それが駆動されると、駆動源によって提供される出力トルクτ_ｏに対向する慣性を経験する。慣性は、ジョイントの内部または外部にある多数の要因によって引き起こされ得る。慣性の原因の例として、重力によるジョイントの重量が挙げられる。別の例示的な原因は、ジョイントによって支持される、またはジョイントと接合される物体の重量である。慣性の存在により、その出力ｑ_ｏにおけるジョイントの構成は、基準構成ｑ_ｒｅｆとは異なってもよい。剛性であるジョイントについては、慣性はジョイントｑ_ｏの出力構成に影響を与えない。従って、剛性のジョイントについては、出力構成ｑ_ｏと所望の構成ｑ_ｒｅｆとの間に差異はない。

センサー２０８は、ジョイントの出力構成ｑ_ｏを測定するために提供される。センサー２０８は、図１に図示したセンサー１１６のうちの一つに対応し得る。従って、センサー２０８は、ロボットアーム１００のジョイント内に含まれてもよい。一実施例では、出力構成ｑ_ｏは、ジョイントの出力位置である。従って、センサー２０８は、線形または回転式エンコーダーなどの位置決めセンサーであり得る。この実施例では、センサー２０８は、それが構成されるジョイントの位置を測定し、この測定値を示す電気信号を構成コントローラー２０２に送信するように構成される。この電気信号の透過率は、更新値基準トルクτ_ｒｅｆを計算するために構成コントローラー２０２によって使用されるフィードバックループを形成する。

一般に、フィードバック項は、ジョイントの所望の構成とその実際の構成の間の偏差を示し、ジョイントを定常状態からさらに取り得る外乱を補償するために、制御システム内で使用される。従って、センサー２０８からフィードバックを組み込む基準トルクτ_ｒｅｆの値は、連続データ測定値がセンサー２０８から取得されるにつれて、連続的に更新され得る。フィードバックループは、構成コントローラー２０２に提供される前に、センサー２０８から取得される未加工の出力値をマニピュレートするための一つまたは複数のフィルター、積分器、または微分器をさらに含み得る。

上述のように、図２に図示されるループによって制御されるジョイントは、「剛性」としてモデル化される。剛性のジョイントについては、入力トルク（すなわち、駆動源が位置するドライブトレインの第一の端部で、または近位の位置で測定されるトルク）は、出力トルク（すなわち、ジョイントが位置するドライブトレインの第二の端部で、または近位の位置で測定されるトルク）に比例する。

ジョイントについて測定される出力トルクは、以下の式によって特徴付けられ得る：
τ_ｏ＝Ｋ（ｑ_ｉ－ｑ_ｏ）

式中、Ｋは、ドライブトレインのばね定数または剛性であり、ｑ_ｉは、ジョイントの入力構成であり、ｑ_ｏは、ジョイントの出力構成である。一実施例では、以下の図４から図６に示すように、ｑ_ｉは、ドライブトレインの第一の端部で測定され、ｑ_ｏは、ドライブトレインの第二の端部で測定される。従って、出力トルクは、ドライブトレインの伸長（ｑ_ｉ－ｑ_ｏ）と剛性Ｋとの間の関係によって特徴付けられる。いくつかの実施例では、伸長と剛性Ｋとの間の関係は直線的である。他の実施例では、この関係は非線形である。

本明細書で参照されるばね定数Ｋは、さもなければばね係数と呼んでもよい。ばね定数（またはばね係数）は、全ての伸長値に対して、必ずしも定数値を有しないことに留意されたい。実際に、いくつかの実施例では、ばね係数は、伸長値の連続関数であり、一部の他の実施例では、ばね係数は、離散値のセットから選択され、各離散値は、それぞれの伸長値の範囲と関連付けられる。

上述のように、ロボットシステム内のジョイントを駆動するための機構は、弾性の関連要素を含み、非線形剛性特性をもたらす。この弾性は、特に高調波駆動の特徴である。図３は、ロボットアームの弾性ジョイントに対する例示的な非線形剛性特性を示すグラフである。グラフは、ｘ軸上の伸長とｙ軸上の出力トルクをプロットする。出力トルクは、ジョイントが位置するドライブトレインの第二の端部に位置するセンサーによって測定され得る。出力トルクを測定するためのセンサーは、ゲージセンサーであり得る。ゲージセンサーは、機械的歪みによる電圧の変化を測定するように構成される。伸長は、入力構成ｑ_ｉをジョイントの出力構成ｑ_ｏから差し引くことによって計算される。図４～６では、入力構成は、駆動源が位置するドライブトレインの第一の端部に位置する第一のセンサーによって測定される。出力構成は、ジョイントが位置するドライブトレインの第二の端部に位置する第二のセンサーによって測定される。

図３では、出力トルクτ_ｏはＮｍで測定され、伸長εは度で測定される。代替的な実施例では、伸長εはラジアンで測定され得る。これらの実施例の両方において、入力および出力構成を測定するための第一および第二のセンサーは、回転位置決めセンサーである。さらなる実施例では、伸長は、線形位置決めセンサーから得られた測定値を使用して計算され得る、線形測定値であり得る。この実施例では、伸長はメートルで測定され得る。さらなる実施例では、伸長は、無次元であり得る。伸長の無次元測定値の例は、パーセンテージとしてである。

図３に図示したグラフは、五つの別個の領域３０２～３１０を含む。領域３０２および３１０は、ドライブトレイン伸長と出力トルクτ_ｏとの間の直接比例関係を示す。すなわち、ドライブトレインの伸長が増加するにつれて、出力トルクは一定割合で増加する。これは、図２を参照して上述したジョイントの理想的な性能を表す。領域３０２は、伸長の負の値に対するジョイント性能を示し、領域３１０は、伸長の正の値に対するその性能を示す。領域３０２および３１０にプロットされたグラフの勾配は、関連する伸長値の範囲内のドライブトレインの剛性Ｋを表す。

領域３０６は、ドライブトレインの剛性を表す線の勾配がほぼゼロである、バックラッシュの現象を表す。上述したように、バックラッシュは、方向が変化するときに経験される機構における損失の動きを指し、ドライブトレインの対向する構成要素間のギャップまたはクリアランスによって引き起こされる。高調波駆動の場合、バックラッシュは、円形スプラインとフレックススプラインとの間のクリアランス、および／またはトルク下でのフレックススプラインの弾性変形の結果である。この現象は、典型的には、伸長値の領域内で発生する。図３に図示した例では、この伸長領域の値は－０．１と０度の間である。当然のことながら、異なるジョイントについては、バックラッシュ領域は、異なる範囲の伸長値で現れる。グラフのこの領域では、測定されるセンサートルクはゼロであり、伸長が－０．１と０度の間で増加するにつれてゼロのままである。従って、伸長の－０．１と０度との間のジョイント方向の反転の間、トルクは駆動源からジョイントへドライブトレインを通して伝達されない。

領域３０４および３０８は、移行ゾーンである。これらのゾーンでは、ドライブトレインは、バックラッシュ領域と意図される性能の領域との間で移行している。これらの領域の間、伸長とセンサートルクの間に非線形関係がある。

図３に記録される伸長値の範囲全体を通してドライブトレインの代表的な剛性は、グラフの全ての領域を通過する回帰線３１２の勾配によって表される。図３から、弾性ジョイントについて、伸長と出力トルクとの間の関係は完全には直線的ではないことが理解される。すなわち、バックラッシュ領域３０６（ゼロ）における剛性特性の勾配は、領域３０２および３１０におけるその勾配と実質的に異なる。従って、３１２で表される直線は、伸張の全ての値に対するドライブトレインの剛性特性の正確な表現を提供する。

図３に示す非線形特性を示すジョイントの性能は、正確にはモデル化できない。従って、ロボット制御システムにとって、ジョイントの実世界の性能を測定するフィードバックループを組み込み、それらの性能測定値をフィードバック項として使用して、ジョイントを駆動するために使用される基準信号を調整することが重要である。

単一の弾性ジョイントの微分方程式は、以下のように表現され得る：

上の式では、Ｍ、Ｂ、Ｋ、Ｄ、Ｆ_ｉおよびＦ_ｏは制御変数である。Ｍはドライブトレインの出力における慣性であり、Ｂ_０はその入力における（すなわち、駆動源の出力における）慣性であり、Ｋはドライブトレインのばね定数であり、Ｄは減衰項である。従って、Ｄはドライブトレインにおける弾性に起因して失われるエネルギーを表す。Ｆ_ｉは入力トルクの外乱である。入力トルクの外乱は、静止摩擦に起因し得る。例えば、Ｆ_ｏは、出力トルクの外乱である。一実施例では、出力トルクの外乱は、ロボットアーム内の他のジョイントによって引き起こされる。

上述の制御変数に加えて、微分方程式は、弾性ジョイントの四つの状態変数、すなわち、位置、速度、加速、およびトルクを観察する。これらの変数は、ドライブトレインの入力または出力のいずれかで定義され得る。上記の方程式では、ジョイントｑ_ｏの出力構成は、ジョイントの出力位置である。従って、

は、それぞれ時間に対する出力位置の一次および二次導関数である。すなわち、

はそれぞれ、ドライブトレインの第二の端部での速度および加速度である。従って、

は、ドライブトレインの第一の端部における位置、速度、および加速である。上記の微分方程式は、ドライブトレインの第一および第二の端部に位置する位置およびトルクセンサーを使用して上述の状態変数を測定することによってパラメーター化することができる。速度および加速度は、時間に対する位置の導関数であり、位置決めセンサーから測定される位置測定値から計算することができる。トルクデータ値は、従来、トルクセンサーの測定値から得る。

図４は、ロボットアーム内のジョイントの構成を制御するための全状態制御スキームを示すブロック図である。図４に図示した特定の例では、ジョイントの構成はその位置である。制御スキームは、図１を参照して記載されるジョイントコントローラー内に構成され得る。制御スキームは、相互接続回路によって互いに電気的に接続されるトルクコントローラー４０２および制御ループ４０４を含む。ジョイントコントローラー４０２は、ロボットアームのジョイント４０６に接続するための一つまたは複数の電気的接続を含む。ロボットアームは、図１に示すアーム１００に対応してもよく、ジョイント４０６は、ジョイント１０４ａ～ｅのうちの一つまたは複数に対応し得る。

トルクコントローラー４０２は、駆動源によって生成され、ドライブトレインによってジョイント４０６に送信される入力トルクτ_ｉの値を計算するように構成される。制御ループ４０４は、ジョイント４０６からセンサーデータを受信し、このセンサーデータをフィードバックとして使用して基準トルク値τ_ｒｅｆを計算するように構成される。

トルクコントローラー４０２は、駆動源４０８および駆動源コントローラー４１０を含む。駆動源４０８は、ロボットアームのジョイント４０６および駆動源コントローラー４１０の両方に接続される。駆動源４０８は、ロボットアームのジョイント４０６を駆動するドライブトレインに入力トルクτ_ｉを供給するように構成される。一実施例では、駆動源４０８はモーターである。他の実施例では、駆動源４０８は、油圧もしくは空気アクチュエーター、または任意の代替電源であり得る。駆動源コントローラー４１０は、駆動源の性能、およびそれゆえジョイント４０６周りの入力トルクを制御するように構成される。

駆動源４０８および駆動源コントローラー４１０は、コントローラーから駆動源へ通過するフィードフォワードループ、および駆動源からコントローラーへ通過するフィードバックループによって相互に電気的に接続される。駆動源がモーターである実施例では、フィードフォワードループは、コントローラーがモーターに三相電圧供給を提供することを可能にする。この実施例では、フィードバックループによって、モーターがコントローラーに三相電流フィードバックを供給することが可能になる。フィードバックループは、モーターに供給される電圧、すなわち電流を変更するために使用する。従って、フィードバックループは、意図されたトルクがモーターによって生成されることを保証する。

トルクコントローラー４０２は、ジョイント４０６の性能を示す一つまたは複数のパラメーターを測定するための複数のセンサーをさらに含む。センサーはさらに、これらの電気信号が制御ループ４０４に送信され得るように、それらの測定値に関連付けられる電気信号を生成するように構成される。一実施例では、センサーはジョイント４０６内に含まれる。別の実施形態では、センサーはジョイント４０６に外部的であるが結合される。複数のセンサーは、図１に示すセンサー１１６に対応し得る。図４では、複数のセンサーは、第一のセンサー４１２、第二のセンサー４１４、および第三のセンサー４１６を含む。

第一のセンサー４１２および第二のセンサー１１４は、ジョイント４０６の周りの構成を測定するように構成される。上述のように、ジョイントの構成は、ジョイントの位置であり得る。この実施例では、センサーはエンコーダーであり得る。エンコーダーは、結合されるジョイントの動きに応じて、線形エンコーダーまたは回転式エンコーダーのいずれかであり得る。第一のセンサー４１２は、ジョイント４０６の入力構成を測定するように構成される。入力位置は、そのジョイントを駆動するよう構成される駆動源の出力で測定される。従って、第一のセンサー４１２は、駆動源４０８が位置する、ドライブトレインの第一の端部に、またはその近位の位置に位置し得る。第二のセンサー４１４は、ジョイント４０６で出力構成を測定するように構成される。出力構成は、ジョイントへの入力で測定される。従って、第二のセンサーは、ジョイント４０６が位置するドライブトレインの第二の端部にまたは近位の位置に位置する。第三のセンサー４１６は、ジョイント４０６の周りの出力トルクを測定するように構成される。従って、第三のセンサーはまた、ドライブトレインの第二の端部に位置する。センサー４１２、４１４、４１６は、ジョイントの性能を示すデータを連続的に感知するように構成され得る。データのこの連続感知は、定期的な間隔で行われてもよい。すなわち、ジョイントからのデータの感知は、所定のサンプリング速度でセンサーによって行われてもよい。このサンプリング速度は、図１に示すコマンドインターフェイス１２４などで構成可能とし得る。

一つまたは複数のセンサーに加えて、トルクコントローラー４０２は、それぞれのセンサーから受信した電気信号をフィルターリングするための一つまたは複数のセンサーフィルターを含む。フィルターは、複数のデータ値からの情報を組み合わせて、ジョイントの性能を効率的に監視するために提供される。一実施例では、フィルターは、時間フィルターであり、これは、それぞれの新しいデータ値に対するそれらの情報を段階的に更新するために使用される。第一のフィルター４１８は、第一のセンサー４１２から受信した電気信号をフィルターリングするように構成される。第一のセンサー４１２が入力位置決めセンサーである場合、第一のフィルター４１８は、入力位置値をフィルターリングするように構成される。従って、第二のフィルター４２２は、第二のセンサー４１４に電気的に接続され、そのセンサーから受信した電気信号をフィルターリングするように構成される。従って、第二のセンサー４１４が出力位置決めセンサーである場合、第二のフィルター４２２は、出力位置値をフィルターするように構成される。第三のフィルター４２６は、第三のセンサー４１６に電気的に接続される。従って、第三のフィルター４２６は、出力トルク値をフィルターするように構成される。

トルクコントローラー４０２は、第一のセンサー４１２と第二のセンサー４１４からそれぞれフィルターリングされた導関数値を導出するための第四のフィルター４２０と第五のフィルター４２４とをさらに含む。上述のように、センサー４１２、４１４、４１６によるデータの感知は、所与のサンプリング速度で行われてもよい。第四および第五のフィルター４２０、４２２は、センサーデータが測定されるサンプリング速度の記録を維持するように構成される。速度は、時間に対する位置の一次導関数である。従って、第一および第二のセンサー４１２、４１４が入力および出力位置決めセンサーである場合、第四および第五のフィルター４２０、４２２は、所定のサンプリング速度および連続的にサンプリングされたデータ値を使用して、ジョイントの入力および出力速度値をそれぞれ計算するように構成される。

一つまたは複数のセンサーに電気的に接続されるだけでなく、一つまたは複数のセンサーフィルターは、着信相互接続回路４２８に電気的に接続される。着信相互接続回路４２８はさらに、制御ループ４０４に接続される。従って、相互接続回路４２８は、センサーデータをトルクコントローラー４０２から制御ループ４０４に送信するように構成される。従って、センサーデータは、制御ループ４０４へのフィードバックとして提供され得る。一実施例では、相互接続回路は、Ｅｔｈｅｒｃａｔバスなどの通信バスである。

制御ループ４０４は、駆動源４０８に提供される基準トルクτ_ｒｅｆを計算するように構成される。基準トルクτ_ｒｅｆは、基準構成ｑ_ｒｅｆおよび測定されるセンサーデータから導出された複数のフィードバックループを使用して計算することができる。制御ループ４０４は、第六のフィルター４３０、構成コントローラー４３２、動的トルク観測器４３４、および重力補償器４３６を含む。

第六のフィルター４３０は、構成コントローラー４３２に接続され、（制御ユニット１１８からなど）一つまたは複数の着信基準構成値ｑ_ｒｅｆを受信するように構成される。第六のフィルター４３０はさらに、これらの値をフィルターして、構成コントローラー４３２への入力として提供するように構成される。フィルター４３０はまた、この基準構成ｑ_ｒｅｆの一次導関数の値を計算するように構成される。基準構成がジョイント４０６の基準位置である場合、この基準構成の（時間に対する）一次導関数は、基準速度である。第六のフィルター４３０は、トルクコントローラー４０２のフィルター４１８、４２０、４２２、４２４、４２６に対応し得る。すなわち、位置フィルターは、複数のデータ値からの情報を組み合わせて、入力位置値を効率的に監視し、保存された所与のサンプリング速度および連続的にサンプリングされる基準位置値を使用して、基準速度値を計算するように構成され得る。

制御ループは重力補償器４３６をさらに含み、これは第六のフィルター４３０の出力と着信相互接続回路４２８の両方と電気的に接続される。重力補償器４３６は、第六のフィルター４３０から基準構成データを受信し、相互接続回路４２８によって第三のセンサー４１６らトルク値を出力するように構成される。重力補償器４３６は、第六のフィルター４３０から得られたデータを使用して、予期トルク値を計算するようにさらに構成される。次いで、重力補償された測定されるトルクの値を、出力トルクτ_ｏから予期トルクを差し引くことによって、重力補償器４３６によって計算することができる。この重力補償された測定されるトルクは、動的トルク観測器４３４に提供される。

重力補償された測定されるトルクに加えて、動的トルク観測器４３４は、相互接続回路４２８から入力構成、出力構成、および出力構成の一次導関数の値を受信するように構成される。動的トルク観測器４３４は、その受信された入力から動的トルク値τ_{ｄｙｎａｍｉｃ}を計算するようにさらに構成される。動的トルク値τ_{ｄｙｎａｍｉｃ}は、動的トルク観測器４３４によって構成コントローラー４３２に送信される。構成コントローラー４３２はまた、第六のフィルター４３０および相互接続回路４２８に電気的に接続される。従って、構成コントローラー４３２は、τ_{ｄｙｎａｍｉｃ}に加えて、入力および出力構成、ならびにこれらの構成の一次導関数を、着信相互接続回路４２８から受信するように構成される。位置コントローラーはさらに、第六のフィルター４３０からこの値の基準構成および一次導関数を受信するように構成される。これらの受信値から、構成コントローラー４３２は、トルクコントローラー４０２に提供される基準トルクτ_ｒｅｆの値を計算する。基準トルクτ_ｒｅｆは、送信相互接続回路４３４によってコマンドループ４０４からトルクコントローラー４０２に送信される。着信相互接続回路４２８と同様に、送信相互接続回路４３４は、Ｅｔｈｅｒｃａｔバスなどの通信バスであり得る。

図５は、ロボットジョイントの構成を制御するための制御アルゴリズム５００を示し、トルクセンサーデータをフィードバック項として使用して入力トルクτ_ｉの値を計算するブロック図である。位置制御アルゴリズムは、例えば、図４に図示する構成コントローラー４３２内に含まれてもよい。位置制御アルゴリズムは、基準構成ｑ_ｒｅｆ（例えば、制御ユニット１１８から受信される）および複数のフィードバック項を使用して、駆動源に提供される入力トルク値τ_ｉを計算する。基準構成ｑ_ｒｅｆは、最初は第一の伝達関数５０２によって乗算され、第一の合計点５０４に提供される。第一の伝達関数５０２は、ｑ_ｒｅｆの未加工の入力値を操作するために提供される第一のゲインを含む。

制御アルゴリズム５００は、駆動源に関連付けられるゲインＧ（複数可）を含む第二の伝達関数５０６をさらに含む。駆動源がモーターである実施例では、ゲインＧ（ｓ）はモーターゲインである。モーターゲインＧ（ｓ）の値は、ジョイントの物理量に依存する。物理的トルクτ_ｐおよび入力トルクτ_ｉの値は、第二の伝達関数５０６への入力として提供される。第二の伝達関数５０６に印加される物理的トルクτ_ｐは、ジョイントシステムに対する現実の外乱を表す。このトルクは電子的には存在しない。第二の伝達関数５０６の出力は、出力位置ｑ_ｏ、入力位置ｑ_ｉ、および出力トルクτ_ｏである_。これらの出力は、ジョイントおよびそのそれぞれのドライブトレインに位置するセンサーから取得される測定パラメーターである。センサーは、例えば、図４に示すセンサー４１２、４１４、４１６に対応し得る。

伝達関数５０６からの、ジョイントで測定される三つの出力ｑ_ｏ、ｑ_ｉ、τ_ｏは、入力トルクτ_ｉを計算するために使用されるフィードバック項として提供される。各測定パラメーターは、関連するフィードバックループとともに提供される。各フィードバックループは、それが関連付けられている出力値に対応する伝達関数を含む。例えば、測定される出力構成ｑ_ｏに対するフィードバックループは、第三の伝達関数５０８を含む。図５に図示した例では、第三の伝達関数は、関連する一つのゲインＫ_ｏを含む。代替的な実施例では、第三の伝達関数５０８は、複数のゲインを含む。第三の伝達関数５０８は、出力構成および出力構成

の一次導関数の両方の操作された値を計算して、合計点５１０に提供するように構成される。伝達関数が複数のゲインを含む例では、伝達関数５０８への入力として提供される出力構成の各導関数に対して、ゲインが提供され得る。従って、第三の伝達関数が、出力構成と出力構成の一次導関数の両方の操作された値を計算するように構成される場合、第三の伝達関数５０８は、二つのゲインを含み得る。さらなる代替的な実施例では、第三の伝達関数５０８は、行列を含んでもよく、行列の各要素は、特定の入力変数を出力変数に関連付ける関数である。

測定される入力構成ｑ_ｉのために提供されるフィードバックループは、第四の伝達関数５１２を含む。第三の伝達関数５０８と同様に、図５に例示される例では、第四の伝達関数は、一つの関連するゲインＫ_ｉを含む。代替的な実施例では、第四の伝達関数５１２は、複数のゲインを含む。具体的には、第四の伝達関数が、入力構成および入力構成の一次導関数の両方の操作された値を計算するように構成される場合、第四の伝達関数５１２は、二つのゲインを含んでもよい。さらなる代替的な実施例では、第四の伝達関数５１２は、行列を含んでもよく、行列の各要素は、特定の入力変数を出力変数に関連付ける伝達関数である。

第四の伝達関数５１２は、入力構成および入力構成

の一次導関数の両方の操作された値を計算して、合計点５１４に提供するように構成される。出力トルクτ_ｏに提供されるフィードバックループは、関連するゲインＫ_τを有する第五の伝達関数５１６を含む。従って、出力トルクτ_ｏの操作された値は、合計点５１４に提供される。ゲインＫ_ｏ、Ｋ_ｉ、Ｋ_τは、所定の理論データまたは実験データから取得され得る、所与の連続値または定数値であり得る。ゲインは、ロボットアーム用の制御システム内に非一時的形態で格納され得る。ゲインは、物理的測定データに適用されてもよく、制御アルゴリズムの性能に影響を与えるために使用される。

フィードバックループは、合計点５１０、５１４で一緒に合計され、合計点５０４で入力トルクτ_ｉを計算するために使用される。すなわち、合計点５０４は、操作された基準構成ｑ_ｒｅｆおよび操作されたセンサーデータフィードバックを入力として受信し、入力トルクτ_ｉを出力する。ドライブトレインの入力および出力におけるジョイントの構成を知ることにより、制御システムは伸長ドライブトレインを決定することができる。従って、ジョイントは、その伸長に依存して制御され得る。

図５に図示した制御アルゴリズムを使用して、コントローラーによって計算された入力トルクτ_ｉの値は、以下の式によって表される：

上の式では、ｑ_ｒｅｆは、アームの各ジョイントについて計算される基準構成である。構成は、各ジョイントの動きに応じて、角または直線であり得る。ｋ_ｐｏ、ｋ_ｐｉ、ｋ_ｄｏおよびｋ_ｄｉは、第一および第二の入力のゲインである。ｋ_ｄｏおよびｋ_ｐｉは、それぞれ入力および出力構成の比例ゲインである。ｋ_ｄｏおよびｋ_ｄｉはそれぞれ、入力および出力構成の微分ゲインである。基準および測定される構成の一次導関数

は、それぞれの速度ω_ｒｅｆ、ω_ｉ、ω_ｏと同義である。

図５に示す制御システムは入力トルクτ_ｉを計算するために、五つの測定されるフィードバック項

を必要とする。しかしながら、測定される出力トルクτ_ｏは、ジョイントの性能の正確な表現を提供するのに必ずしも使用されない。これは、高調波駆動などのバックラッシュを経験するドライブトレインを有するジョイントに特に関連する。一般に、ドライブトレインの伸長は、そのジョイントに印加されるトルクの変動とともに直線的に変化する。上述のように、非線形ジョイントのバックラッシュ領域では、ジョイント伸長が変化するにつれて、測定されるセンサートルクはほぼゼロである。従って、バックラッシュ領域の間は、トルクセンサーは測定されるトルクの値を提供するために使用できない。この領域中にコントローラーに提供される出力トルクを決定するより正確な方法が必要である。

上述のように、ロボットジョイントの出力トルクは、以下の式によって特徴付けられ得る。：
τ_ｏ＝Ｋ（ｑ_ｉ－ｑ_ｏ）

式中、Ｋは、ジョイントのばね定数であり、ｑ_ｉはジョイントに関する入力構成であり、ｑ_ｏはジョイントに関する出力構成である。前述したように、ｑ_ｉ－ｑ_ｏは、ドライブトレインの伸長を表し、ｑ_ｉおよびｑ_ｏは、ジョイントおよびその関連するドライブトレインに連結されるセンサーを使用して両方を測定することができる。従って、ジョイントの出力トルクは、入力および出力ジョイント構成に対する測定されるセンサーデータを使用して計算することができる。この出力トルクの計算は、トルクセンサーから得られた出力トルクの測定値を置き換えるために使用することができる。

図６は、トルクセンサーデータをフィードバック項として使用せずに、ジョイントの構成を制御するための制御アルゴリズムを示すブロック図である。代わりに、入力および出力ジョイント構成に出力トルクをリンクする上記方程式を使用して、出力トルクの値を計算して、制御システムにフィードバックを提供する。構成制御アルゴリズムを実施するためのコントローラーは、図４に図示したスキームに広く対応するが、第三のセンサー４１６から取得されたトルクセンサーデータを除外する制御スキーム内に実装され得る。制御アルゴリズムは、図４に示すコントローラー４３２と同様のコントローラーによって実装されるように構成される。

制御アルゴリズムは、三つの伝達関数、第一の伝達関数６０２、第二の伝達関数６０６、および第三の伝達関数６１０を含む。アルゴリズムは、第一の合計点６０４および第二の合計点６０８をさらに含む。図５に示すアルゴリズムと同様に、制御アルゴリズムは、基準構成ｑ_ｒｅｆを受信し、この基準構成および複数のフィードバック項を使用して、入力トルク値τ_ｉを計算する。

基準構成ｑ_ｒｅｆは、図１のユニット１１８などの制御ユニットから受信される。基準構成ｑ_ｒｅｆは、第一の伝達関数６０２への入力として提供される。第一の伝達関数６０２は、以下でより詳細に記述される。第二の伝達関数６０６は、駆動源に関連付けられるゲインＧ（複数可）を含む。物理的トルクτ_ｐおよび入力トルクτ_ｉの値は、第二の伝達関数６０６への入力として提供される。第二の伝達関数６０６の出力は、出力構成ｑ_ｏおよび入力構成であるｑ_ｉ。これらの出力は、ジョイントおよびそのそれぞれのドライブトレインに位置するセンサーから取得される測定パラメーターである。

一実施例では、図６に図示するように、第三の伝達関数６１０は二つの値Ｋ_τおよびＫ_φを含む。Ｋ_φは、ドライブトレインのばね定数であり、上で言及したＫと同義語であり得る。Ｋ_φは、以下でより詳細に説明される。Ｋ_τは、ｑ_ｏおよびｑ_ｉの未加工値を操作するために提供されるゲインである。代替的な実施例では、第三の伝達関数６１０はマトリクスである。この実施例では、マトリックスの各要素は、特定の入力変数を出力変数に関連付ける関数である。

図６の制御アルゴリズムを実施するコントローラーを使用してロボットジョイントを制御する方法を、図７に示す。ステップ７０２で、コントローラーは、第一のセンサーから第一の入力を受信する。第一の入力は、ジョイントｑ_ｉの入力構成を示す。第一のセンサーは、図４の第一のセンサー４１２に対応し得る。図４を参照して上述したように、第一のセンサー４１２は、駆動源４０８が位置するドライブトレインの第一の端部に位置し得る。

ステップ７０４で、コントローラーは第二のセンサーから第二の入力を受信する。第二の入力は、ジョイントｑ_ｏの出力構成を示す。出力構成は、図４の第二のセンサー４１４に対応し得る第二のセンサーから受信される。一実施例では、第二のセンサー４１４は、ジョイントが位置するドライブトレインの第二の端部に位置し得る。

ステップ７０６で、コントローラーは、第一および第二の入力を使用して、ジョイント周りの出力トルクｔ_ｏの値を計算するように構成される。ステップ７０８で、コントローラーは、出力トルクの計算値を使用して、ジョイントに印加される入力トルクの値を計算する。一実施例では、入力トルクを計算することは、出力トルクｔ_ｏの計算値から重力の作用を補償するトルク項を差し引くことを含む。入力トルクを計算するために重力の作用を補償するトルク項を使用することは、その動作中の重力によるジョイントのドリフトを防止する。

図６では、第一の入力と第二の入力の両方が第三の伝達関数６１０への入力として受信される。一実施例では、第一の入力および第二の入力はそれぞれ、第三の伝達関数６１０に提供される前に、ゲインによってスケーリングされる。この実施例では、第一の入力がスケーリングされるゲインは、第二の入力がスケーリングされるゲインとは異なってもよい。第三の伝達関数６１０は、ジョイントの出力構成ｑ_ｏおよび入力構成ｑ_ｉの受信した値を使用して、出力トルクτ_ｏの値を計算するようにさらに構成される。この出力トルクτ_ｏは、トルクセンサーによって測定されるであろうトルクの値とは異なってもよい、トルクの理想的な値に対応する。すなわち、ジョイントがバックラッシュを経験している場合、ジョイントτ_ｏの出力トルクの測定値はゼロとなる。しかし、入力および出力構成値を使用して計算される出力トルクτ_ｏの値は、これらの条件下では非ゼロの値を有する。第三の伝達関数６１０からの出力として提供される出力トルクτ_ｏの計算値は、合計点６０４への入力として提供される。第一の伝達関数６０２の出力はまた、合計点６０４に提供される。

合計点６０４は、第一の伝達関数６０２の出力とともに、出力トルクの計算値を使用して、入力トルクτ_ｉの値を計算することである。この計算は、図７のステップ７０８で実行される。図６に例証される実施例では、第一の伝達関数６０２は、図５に例証される伝達関数５０８および５１２のゲインＫ_ｏおよびＫ_ｉに対応する一つの関連するゲインＫ_ＰＤを含む。代替的な実施例では、第一の伝達関数６０２は、行列を含み、行列の各要素は、特定の入力変数を出力変数に関連付ける伝達関数である。従って、伝達関数６０２は、入力としてｑ_ｏおよびｑ_ｉの測定値を受信し、これらの構成の操作されたバージョンおよびそれらの導関数を合計点６０４に提供するように構成される。別の実施例では、第一の伝達関数６０２は、複数のゲインを含む。伝達関数６０２が、入力としてｑ_ｏおよびｑ_ｉの測定値を受信するように構成される場合、これは二つのゲインを含み得る。この実施例では、出力構成ｑ_ｏを操作するために第一のゲインが提供され、入力構成ｑ_ｉを操作するために第二のゲインが提供される。

図６に記述される制御アルゴリズムは、ジョイントの入力および出力構成を示すことからセンサーデータの連続的なストリームを受信し得る。従って、制御アルゴリズムは、入力トルクτ_ｉの値を繰り返し計算するように構成され得る。入力トルクτ_ｉの連続計算により、制御システムは、発生するときとジョイント性能の外乱を即座に補償することができることを確実にする。一実施例では、この補償は、実質的にリアルタイムで発生し得る。

図６に図示した制御アルゴリズムを使用して、コントローラーによって計算された入力トルクτ_ｉの値は、以下の式によって表される：

上の式では、ｑ_ｒｅｆは、アームの各ジョイントについて計算される基準構成である。構成は、各ジョイントの動きに応じて、角または直線であり得る。Ｋ_ｐｏ、ｋ_ｐｉ、Ｋ_ｄｏおよびＫ_ｄｉは、第一および第二の入力のゲインである。Ｋ_ｐｏおよびｋ_ｐｉは、それぞれ入力および出力構成の比例ゲインである。Ｋ_ｄｏおよびＫ_ｄｉはそれぞれ、入力および出力構成の微分ゲインである。基準および測定される構成

の一次導関数はそれぞれの速度ω_ｒｅｆ、ω_ｉおよびω_ｏと同義である。

上の方程式の第一の項ｑ_ｒｅｆ－ｑ_ｏは、基準構成とジョイントの測定される出力構成との比較を表す。第二の項ｑ_ｒｅｆ－ｑ_ｉは、基準構成とジョイントの測定される入力構成との比較によって計算される。第三の項

は、基準構成の一次導関数と、ジョイントの測定される出力構成の一次導関数との比較によって計算される。第四の項

は、基準構成の一次導関数と、ジョイントの測定される入力構成の一次導関数との比較によって計算される。従って、上の方程式の最初の四つの項は、ジョイントの所望の構成を、測定されるセンサーデータから得られた入力および出力の構成と比較することによって導出される。すなわち、ジョイントの所望の構成とその測定される構成との間の差に依存して、所望の構成に向かってジョイントを駆動する傾向にある入力トルクを計算することができる。上の方程式の最初の四つの項は、関連するゲインとともに、第一の伝達関数６０２からの出力として提供される。

一実施例では、制御アルゴリズム６００によって制御されるジョイントの構成は、ジョイントの位置である。この実施例では、基準構成は、ジョイントの基準、または所望の物理的位置である。入力および出力構成は、それぞれジョイントの入力および出力位置である。

第三の伝達関数６１０内に含まれるばね定数Ｋφは、上で言及したようにＫと同義であり得る。ばね定数Ｋφは、ドライブトレイン伸長ｑ_ｏ－ｑ_ｉの関数である理論的に計算された値である。すなわち、Ｋφは、歴史的理論データまたは実験データから取得され、ロボットアーム用の制御システム内の非一時的形態で保存された、所与の値または値の範囲である。図３を参照すると、Ｋφは出力トルクに対するドライブトレイン伸長をプロットするグラフの勾配によって特徴付けすることができる。

一実施例では、ばね定数Ｋφは、連続関数によるドライブトレイン伸長に関連する。すなわち、Ｋφはドライブトレインの伸長が変化するにつれて、連続的に変化し得る。別の実施例では、ばね定数Ｋφは、定数値を有し得る。すなわち、Ｋφはドライブトレインの伸長が変化するのと同じ値を持つ。あるいは、ばね定数は、値の離散範囲から選択され得る。この実施例では、Ｋφの各離散値は、伸長値の範囲と関連付けられてもよく、伸長値の各範囲は、一つまたは複数の所定の閾値によって定義され得る。ばね定数が三つの別個の値の範囲から選択される実施例を以下に示す：
ｅ＜－０．１度→Ｋφ＝Ｋ１
－０．１＜ｅ＜０度→Ｋφ＝Ｋ２
０度＜ｅ→Ｋφ＝Ｋ３

上の例では、測定される伸長が－０．１度未満であるとき、第一のばね定数Ｋ１が使用される。すなわち、第一の剛性値Ｋ１は、－０．１度の上限閾値によって特徴付けられる伸張値の範囲と関連付けられる。この伸長値の範囲は、図３に示すグラフの領域３０２に対応する。第二の剛性値Ｋ２は、ドライブトレインの伸長が－０．１度～０度の間である場合に使用される。すなわち、第二の剛性値Ｋ２は、－０．１度の下限閾値および０度の上限閾値によって特徴付けられる、伸張値の範囲と関連付けられる。この伸長値の範囲は、図３に図示したグラフの領域３０６に対応する。すなわち、この伸長範囲は、図３に示すように、バックラッシュ領域に対応する。第三の剛性値Ｋ３は、測定される伸長が０度より大きいときに使用される。すなわち、第三の剛性値Ｋ３は、０度の下限閾値によって特徴付けられる伸張値の範囲と関連付けられる。この伸長値の範囲は、図３に示すグラフの領域３１０に対応する。

さらなる実施例では、ばね定数は、五つの異なる値の範囲から選択され得る。これら五つの別個の値には、上記で示される第一、第二、および第三の剛性値Ｋ１、Ｋ２、およびＫ３が含まれる。これに加えて、第四の剛性値Ｋ４は、測定される伸長が－０．１度と等しいか、または－０．１度に近いときに選択され得る。これは、図３に図示したグラフの領域３０４によって示される移行ゾーンに対応する。第五の剛性値Ｋ５は、測定される伸長が０度と等しいか、または０度に近いとき、使用され得る。これは、図３に示すグラフの領域３０８によって示される移行ゾーンに対応する。

図３を参照して上述したように、上述の伸長値の範囲は例示的であり、こうした値の代替範囲は剛性値Ｋ１－Ｋ５と関連付けられ得ることが理解されよう。

ロボット制御システムは、図５に示すコントローラーと図６に示すコントローラーの両方を含んでもよい。すなわち、制御システムは、フィードバック項として測定される出力トルクを使用する一つのコントローラーと、この測定値を出力の計算値で置き換える別のコントローラーとを含んでもよい。これら二つのコントローラーを組み合わせて使用して、異なるトルクフィードバック項が、異なる伸張値でより正確な入力を提供するため、非線形剛性特性を有するジョイントを制御することができる。従って、伸張のいくつかの値については、測定されるトルクを観察することが有用であり得、他のもの（例えば、バックラッシュ領域内）については、計算されたトルクを観察することが有用であり得る。

図５および図６に図示したコントローラーは、図８に図示した動的トルク観測器８００で実施され得る。図４を参照して記載したように、動的トルク観測器の目的は、外力を無視しながら、ジョイントの動きおよび／または加速度を考慮するジョイントトルクの値を計算することである。

動的トルク観測器８００は、微分ローパスフィルター８０２、慣性スケーラー８０４、ジョイント剛性スケーラー８０６、第一の重み付けユニット８０８、第二の重み付けユニット８１０、およびハイパスフィルター８１２を含む。合計点８１４、８１６および８１８をさらに含む場合。

微分ローパスフィルター８０２は、出力速度値

（またはω_ｏ）を入力として受信するように構成される。この速度値は、制御アルゴリズム６００によって実行される測定される出力構成ｑ_ｏの微分から取得される。出力速度ω_ｏは、ジョイントを加速するために必要な力を示す慣性項を計算するために使用される。この値は、低周波数では有用であるが、高周波数では過剰なノイズを生成する。従って、ローパスフィルター８０２は、着信出力速度値を受信し、受信値から高周波値をフィルターリングするように構成される。ローパスフィルター８０２から出力される値は、慣性スケーラー８０４への入力として提供される。慣性スケーラー８０４は、フィルターリングされた出力速度値に固定慣性を適用し、慣性トルクの値を計算するように構成される。

動的トルク観測器は、図５に示すようにコントローラーおよび図６に示すようにコントローラーの両方からの入力を使用して、出力トルクτ_ｏの更新値を計算するようにさらに構成される。合計点８１４およびジョイント剛性スケーラー８０６は、減算された重力によるトルクの構成要素上の図６を参照して記載されるように、出力トルクτ_ｏの計算値を提供するよう使用される。重力による出力トルクの成分は、理論値とすることができる。このトルクは、それが構成されるジョイントおよびアームの幾何学的形状、質量および構成を使用して計算され得る。τ_ａｄｊの初期値は、第二の重み付けユニット８１０によって重み付けされ、得られた操作された値は、合計点８１６に提供される。

重み付けユニット８０８、８１０は、合計点８１６に提供される各フィードバック項の割合を修正するために提供される。第一の重み付けユニット８０８は、第一の値１－αを含む。第二の重み付けユニットは、第二の値αを含む。αは、所定のパラメーターであり、ここで１≧α≧０である。一実施例では、αは、ドライブトレイン伸長の関数である。すなわち、αは連続関数によるドライブトレイン伸長に関連するものであってもよく、または定数値を有してもよく、またはＫφに対して上述したように、離散範囲の値から選択され得る。重み付けユニット８０８、８１０から出力される、計算される／測定される出力トルクの操作された値は、合計点８１６に提供される。

出力トルクは、動的トルク（加速、アクチュエータートルク、および摩擦によって引き起こされる）と外部印加トルクの二つの構成要素を含むと仮定される。動的トルクを観察するには、出力トルクの外部適用トルク成分を除去する必要がある。これは、ハイパスフィルター８１２の追加によって達成される。ハイパスフィルター８１２は、合計点８１６の出力から計算された出力トルクの新しい値を受信し、この信号をフィルターリングして外部トルク成分を除去する。次に、ハイパスフィルター８１２の操作された出力を、合計点８１８へ提供することができる。合計点８１８はさらに、慣性スケーラー８０４から慣性トルク値を受信し、動的トルクτ_{ｄｙｎａｍｉｃ}の変更された値を出力として提供するように構成される。

上述の実施例では、計算された出力トルクに使用される入力構成ｑ_ｉは、ドライブトレインの第一の端部に位置する、または近位の位置にあるセンサーから取得される測定構成である。別の実施例では、入力構成は、出力ジョイント構成が測定される時間の前の時間に要求される基準ジョイント構成の値を使用して計算され得る。理想的には、「剛性」ジョイントの場合、以前の基準構成は、ジョイントの入力構成と同一である。従って、この実施例では、入力ジョイント構成を測定するためのセンサーは必要とされず、ジョイントのコンパクトさが増大する。

図２～６は、上述のように、ジョイントによって生成される力がトルクであることを意味する。従って、図２～６は、回転ジョイントの構成を制御するためのコントローラーを指す。代替的な実施例では、制御されるジョイントは、直線運動を提供するように構成され得る。この実施例では、対応する力は、歪ゲージまたは他の力センサーを使用して測定および計算することができる。

図６に図示した制御アルゴリズムを使用して、トルクセンサーデータをフィードバック項として使用せずに、外部からの力および重力による力に応答する、ロボットアーム内のジョイントの構成を制御することができる。従って、このアルゴリズムを使用する制御システムは、ジョイントに対する出力トルクを測定するためにトルクセンサー（図４の第三のセンサー４１６など）を必要としない。（図５に図示する追加のコントローラーとは対照的に）図６に図示する構成コントローラーのみを含むロボット制御システムについては、出力トルクを測定するためのセンサーの除去は、よりコンパクトなジョイント設計をもたらし、それによって、ジョイントのアセンブリー工程の数および計算コストを低減する。また、システム内のノイズによって生成される信号の外乱は、センサー入力の減少の結果として低減され得る。

上記に加えて、図６に図示した制御アルゴリズムはまた、ジョイントを駆動するためのドライブトレインが低剛性を提供するときに、ジョイント制御システムの不安定性を最小化するためのソリューションを提示し得る。つまり、フィードバック項の数を最小化することによって、これらの項に関連するゲインの数を最小化することができる。従って、これらの利益の安定性を最大化するために必要な調整は低減される。従って、制御システムを使用して、所与のジョイント応答を得るために駆動源によって送達される必要のある所望のトルクの良好な推定値を提供することができる。

本明細書によって、本出願人は、本明細書に説明される各個々の特徴および二つ以上のかかる特徴の任意の組み合わせを、かかる特徴または組み合わせが、当業者に共通する一般知識に照らして、全体として本明細書に基づいて行うことができるような程度まで、かかる特徴または特徴の組み合わせが、本明細書に開示する任意の問題を解決するかにかかわらず、かつ特許請求の範囲を限定することなく、分離して開示する。本出願人は、本発明の態様が、任意のかかる個々の特徴または特徴の組み合わせからなり得ることを示す。前述の説明を考慮すると、本発明の範囲内でさまざまな修正を行うことができることは当業者には明らかであろう。

Claims (13)

1. 手術用ロボット内のジョイントの構成を制御するためのコントローラーであって、前記ジョイントは駆動源から前記ジョイントにトルクを伝達するドライブトレインによって駆動され、前記コントローラーが、
   第一のセンサーから前記駆動源の物理的位置を示す第一の入力を受信し、
   第二のセンサーから第二の入力を受信することであって、前記第二の入力が前記手術用ロボット内の前記ジョイントの測定される物理的位置を示すように受信し、
   前記第一の入力および前記第二の入力を使用して、前記ジョイント周りの出力トルクの値を計算し、
   前記出力トルクの値を使用して、前記駆動源によって前記手術用ロボット内の前記ジョイントに印加される入力トルクの値を計算する、
   ように構成される、コントローラー。
2. 前記コントローラーが、指令されるエンドエフェクター姿勢から計算される第三の入力であって、前記ジョイントの所望の物理的位置を示す第三の入力を受信するようにさらに構成される、請求項１のいずれかに記載のコントローラー。
3. 前記入力トルクの値が、
   前記第一の入力と前記第三の入力との間の比較と、
   前記第二の入力と前記第三の入力との間の比較と、
   前記第一の入力の時間に関する一次導関数と前記第三の入力の時間に関する一次導関数との間の比較と、
   前記第二の入力の時間に関する一次導関数と前記第三の入力の時間に関する一次導関数との間の比較と、
   を使って計算される、請求項２に記載のコントローラー。
4. 出力トルクが、前記ドライブトレインは少なくとも部分的に弾性を有し、前記ドライブトレインの伸長値と前記ドライブトレインの剛性値との間の関係によって特徴付けられ、前記出力トルクの値が方程式τ_ｏ＝ｋφ（ｑ_ｉ－ｑ_ｏ）によって表され、式中、ｑ_ｉが前記第一の入力であり、ｑ_ｏが前記第二の入力であり、ｑ_ｉ－ｑ_ｏが伸長値であり、ｋφが前記ドライブトレインのばね係数である、請求項１～３のいずれかに記載のコントローラー。
5. ｋφが連続関数によって（ｑ_ｉ－ｑ_ｏ）に関連し、ｋφの値は（ｑ_ｉ－ｑ_０）の変化に依存して変化する、請求項４に記載のコントローラー。
6. ｋφが、離散範囲の値から選択され、ｋφの各値が、一つまたは複数の所定の閾値によって画定される伸長値の範囲と組み合わされて使用される、請求項４に記載のコントローラー。
7. 前記ばね係数が、前記ドライブトレインの測定される伸長に依存する三つの別個の値から選択され、
   前記測定される伸長の値が第一の所定の閾値を下回る場合、第一の値が前記ばね係数に対して選択され、
   前記測定される伸長の値が前記第一の所定の閾値を上回り、かつ第二の所定の閾値を下回る場合、第二の値が前記ばね係数に対して選択され、
   前記測定される伸長の値が前記第二の所定の閾値を上回る場合、第三の値が前記ばね係数に対して選択される、請求項６に記載のコントローラー。
8. 前記第一の所定の閾値を上回り、かつ第二の所定の閾値を下回る前記伸長値の範囲が、前記ジョイントのバックラッシュ領域に対応する、請求項７に記載のコントローラー。
9. 前記入力トルクの値が、以下の式によって表され、
   

   

   式中、ｑ_ｉは前記第一の入力であり、ｑ_ｏは前記第二の入力であり、ｑ_ｒは前記第三の入力であり、ｋ_ｐｏ、ｋ_ｐｉ、ｋ_ｄｏ、ｋ_ｄｉ、およびｋ_ｔは、前記第一、第二および第三の入力に関連付けられるゲインであり、前記コントローラーは、前記出力トルクの値を繰り返し計算するように構成される、請求項２、又は、請求項２に従属する請求項３～８のいずれかに記載のコントローラー。
10. 前記コントローラーが、動的トルク観測器内に実装され、前記動的トルク観測器が、前記コントローラーによって計算された前記出力トルクの値に重み付けを適用することによって動的トルクの値を計算するように構成され、前記入力トルクの値を計算することが、前記出力トルクの値から重力の作用を補償するトルク項を差し引くことを含む、請求項１～９のいずれかに記載のコントローラー。
11. 前記駆動源がモーターであり、前記ドライブトレインが一つまたは複数のギアを含み、前記ドライブトレインが高調波駆動である、請求項１～１０のいずれかに記載のコントローラー。
12. 前記第二のセンサーが、前記ジョイントが位置する前記ドライブトレインの第二の端部に、またはその近位の位置に位置し、前記第一のセンサーが、前記駆動源が位置する前記ドライブトレインの第一の端部に、またはその近位の位置に位置する、請求項１～１１のいずれかに記載のコントローラー。
13. 手術用ロボット内のジョイントの構成を制御するための制御方法であって、前記ジョイントが、駆動源から前記ジョイントにトルクを伝達するドライブトレインによって駆動され、前記制御方法が、
    第一のセンサーから前記駆動源の物理的位置を示す第一の入力を受信することと、
    第二のセンサーから第二の入力を受信することであって、前記第二の入力が前記手術用ロボット内の前記ジョイントの測定される物理的位置を示す、受信することと、
    前記第一の入力および前記第二の入力を使用して、前記ジョイントに対する出力トルクの値を計算することと、
    前記出力トルクの値を使用して、前記駆動源によって前記手術用ロボットの前記ジョイントに印加される入力トルクの値を計算することと、
    を含む、制御方法。

Images