特開 2022-60279 制御システム及び制御方法、並びにアームシステム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2022060279

(43)【公開日】2022-04-14

(54)【発明の名称】制御システム及び制御方法、並びにアームシステム

(51)【国際特許分類】

   B25J 9/16 20060101AFI20220407BHJP

   A61B 34/30 20160101ALI20220407BHJP

【ＦＩ】

B25J9/16

A61B34/30

【審査請求】有

【請求項の数】14

【出願形態】ＯＬ

(21)【出願番号】P 2022015331

(22)【出願日】2022-02-03

(62)【分割の表示】P 2019502490の分割

【原出願日】2018-01-12

(31)【優先権主張番号】P 2017035965

(32)【優先日】2017-02-28

(33)【優先権主張国・地域又は機関】JP

(71)【出願人】

【識別番号】000002185

【氏名又は名称】ソニーグループ株式会社

(74)【代理人】

【識別番号】100093241

【弁理士】

【氏名又は名称】宮田 正昭

(74)【代理人】

【識別番号】100101801

【弁理士】

【氏名又は名称】山田 英治

(74)【代理人】

【識別番号】100095496

【弁理士】

【氏名又は名称】佐々木 榮二

(74)【代理人】

【識別番号】100086531

【弁理士】

【氏名又は名称】澤田 俊夫

(74)【代理人】

【識別番号】110000763

【氏名又は名称】特許業務法人大同特許事務所

(72)【発明者】

【氏名】松田 康宏

(72)【発明者】

【氏名】新井 淳

(72)【発明者】

【氏名】笠井 栄良

(72)【発明者】

【氏名】黒田 容平

(72)【発明者】

【氏名】小久保 亘

(57)【要約】

【課題】複数のリンクが関節部により連結された多リンク構造のロボット装置を力制御方式により制御する制御システムを提供する。
【解決手段】制御システムは、一般化逆動力学において、多リンク構造体に作用する力と多リンク構造体の加速度との慣性を記述する操作空間上での運動目的及び拘束条件を設定し、運動目的を表す操作空間加速度を実現するために操作空間に作用する仮想力を、ロボット装置の傾斜情報に応じた重力補償を考慮した操作空間バイアス加速度の項を含む操作空間に関する運動方程式に基づいて算出し、仮想力から変換した実在力を基に関節部のトルク指令値を算出する。
【選択図】 図９

【特許請求の範囲】

【請求項1】

アーム装置を制御する制御システムであって、
前記アーム装置が取り付けられた台車の傾きに起因する前記アーム装置の傾斜情報を取得する傾斜情報取得部と、
前記傾斜情報に応じた重力補償を含む力制御方式の演算条件を設定する設定部と、
前記演算条件に従って、前記アーム装置に対する指令値を算出する演算部と、
を具備する制御システム。

【発明の詳細な説明】

【技術分野】

【0001】

本明細書で開示する技術は、複数のリンクが関節部により連結された多リンク構造のアーム装置を力制御方式により制御する制御システム及び制御方法、並びに手術アームシステムに係り、例えば医療分野に適用される手術アーム装置を力制御方式により制御する制御システム及び制御方法、並びに手術アームシステムに関する。

【背景技術】

【0002】

近年、医療分野を含むさまざまな産業分野において、手術アーム装置が普及してきている。少なくとも一部の人的作業を手術アーム装置に置き換えることにより、作業の高精度化、高能率化を実現することができる。

【0003】

手術アーム装置は、例えば複数のリンクが関節部によって互いに連結された多リンク構造体であり、複数の関節部の回転駆動を制御することにより、手術アーム装置全体としての所望の動作が実現される。手術アーム装置並びに各関節部の制御方法として、位置制御と力制御を挙げることができる。位置制御では、関節部のアクチュエータに対して角度などの位置指令値を与え、その指令値に追随するように関節部の駆動が制御される。一方、力制御では、例えば手術アーム装置が作業対象に加えるべき力の目標値を与え、その目標値が示す力を実現するように関節部の駆動（例えば関節部によって発生されるトルク）が制御される。

【0004】

位置制御型の手術アーム装置は、システムの構成が容易であるが、外力に柔軟に応じることが困難である。これに対し、力制御型の手術アーム装置は、システムの構成が複雑化する反面、外力に柔軟に応じることが可能であるため、多様な外界との物理インタラクションを行ないながらタスク（運動目的）を遂行するのに適している。とりわけ、医療用の手術アーム装置は、狭い手術室内で施術者や助手などと至近距離で動作し、対人物理インタラクションを実現することが求められていることから、力制御が好ましいと言うことができる。

【0005】

例えば、複数のリンクを連結し、複数の前記リンクから構成される多リンク構造体の駆動に対して少なくとも６自由度以上の自由度を実現する複数の関節部と、前記関節部の状態に基づいて前記関節部の駆動を力制御する駆動制御部を備える手術アーム装置について提案がなされている（例えば、特許文献１を参照のこと）。この種の手術アーム装置の前記多リンク構造体の先端には、内視鏡や鉗子などの医療用器具からなる先端ユニットが取り付けられる。

【先行技術文献】

【特許文献】

【0006】

【特許文献1】国際公開第２０１５／０４６０８１号

【特許文献2】特表２０１６－５１８１９９号公報

【発明の概要】

【発明が解決しようとする課題】

【0007】

本明細書で開示する技術の目的は、複数のリンクが関節部により連結された多リンク構造の手術アーム装置を力制御方式により制御する制御システム及び制御方法、並びに手術アームシステムを提供することにある。

【課題を解決するための手段】

【0008】

本明細書で開示する技術は、上記課題を参酌してなされたものであり、その第１の側面は、
手術アーム装置を制御する制御システムであって、
前記手術アーム装置の傾斜情報を取得する傾斜情報取得部と、
前記傾斜情報に応じた重力補償を含む演算条件を設定する設定部と、
前記演算条件に従って、前記手術アーム装置に対する指令値を算出する演算部と、
を具備する制御システムである。

【0009】

但し、ここで言う「システム」とは、複数の装置（又は特定の機能を実現する機能モジュール）が論理的に集合した物のことを言い、各装置や機能モジュールが単一の筐体内にあるか否かは特に問わない。

【0010】

前記手術アーム装置は、基本的には、複数のリンクが関節部により連結して構成される多リンク構造体からなる。前記演算条件設定部は、一般化逆動力学において、前記多リンク構造体に作用する力と前記多リンク構造体の加速度との慣性を記述する操作空間上での、前記多リンク構造体の運動目的及び拘束条件を設定する。そして、前記演算部は、前記運動目的を表す前記操作空間加速度を実現するために前記操作空間に作用する仮想力を、前記傾斜情報に応じた重力補償を考慮した操作空間バイアス加速度の項を含む前記操作空間に関する運動方程式に基づいて算出するとともに、前記拘束条件を考慮して前記仮想力を実在力に変換し、前記実在力に基づいて前記関節部を駆動するためのトルク指令値を算出する。

【0011】

また、前記演算部は、前記関節部が理論応答モデルから逸脱しないように、
前記実在力と前記関節部に作用する外トルクに応じて前記理論応答モデルに基づいて算出されるトルク目標値を、前記関節部の外乱トルクを用いて修正して、トルク指令値を算出するようにする。前記演算部は、前記関節部の角速度から前記関節部に作用したトルクを推定し、前記推定したトルクとトルク指令値との差分に基づいて前記外乱トルクを推定することができる。

【0012】

また、本明細書で開示する技術の第２の側面は、
手術アーム装置を制御する制御方法であって、
前記手術アーム装置の傾斜情報を取得する傾斜情報取得ステップと、
前記傾斜情報に応じた重力補償を含む演算条件を設定する設定ステップと、
前記演算条件に従って、前記手術アーム装置に対する指令値を算出する演算ステップと、
を有する制御方法である。

【0013】

また、本明細書で開示する技術の第３の側面は、
複数のリンクが関節部により連結して構成される多リンク構造体と、
前記多リンク構造体の傾斜情報を取得する傾斜情報取得部と、
前記関節部の状態を検出する関節状態検出部と、
前記傾斜情報と前記関節部の状態に応じて算出される指令値に従って前記関節部の駆動を制御する駆動制御部と、
を具備する手術アームシステムである。

【発明の効果】

【0014】

本明細書で開示する技術によれば、複数のリンクが関節部により連結された多リンク構造の手術アーム装置を力制御方式により制御する制御システム及び制御方法、並びに手術アームシステムを提供することができる。

【0015】

なお、本明細書に記載された効果は、あくまでも例示であり、本発明の効果はこれに限定されるものではない。また、本発明が、上記の効果以外に、さらに付加的な効果を奏する場合もある。

【0016】

本明細書で開示する技術のさらに他の目的、特徴や利点は、後述する実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。

【図面の簡単な説明】

【0017】

【図1】図１は、手術アーム装置１００の構成例を模式的に示した図である。

【図2】図２は、手術アーム装置１００が台車２００上に取り付けられた様子を示した図である。

【図3】図３は、手術アーム装置１００が手術台３００に取り付けられた様子を示した図である。

【図4】図４は、手術アーム装置１００を取り付けた手術台３００がヨー回りに傾斜する様子を示した図である。

【図5】図５は、手術アーム装置１００を取り付けた手術台３００がピッチ回りに傾斜する様子を示した図である。

【図6】図６は、手術アーム装置１００の制御システム６００の機能的構成を示した図である。

【図7】図７は、アクチュエータ７００の構成例を示した図である。

【図8】図８は、アクチュエータ７００が理論モデルに従った応答を行なうための制御ブロック図を示した図である。

【図9】図９は、一般化逆動力学演算を利用して力制御を実現する手術アーム装置１００の制御システム９００の機能的構成を示した図である。

【発明を実施するための形態】

【0018】

以下、図面を参照しながら本明細書で開示する技術の実施形態について詳細に説明する。

【0019】

図１には、本明細書で開示する技術を適用することが可能な手術アーム装置１００の構成例を模式的に示している。図示の手術アーム装置１００は、エンドエフェクター１１０を先端に取り付けた手術アーム１２０で構成される。手術アーム装置１００は、例えば、腹腔や胸腔といった内視鏡下手術に利用される医療用若しくは手術用ロボットであり、マスタースレーブ方式システムにおけるスレーブとして動作する。

【0020】

手術アーム１２０は、複数のリンクを各関節部で連結した多リンク構造体からなる。力学的には、各リンクを剛体として扱うことができる。手術アーム１２０が備える軸数（若しくは、関節数）や各軸の自由度構成、リンク数（若しくは、アーム数）などの構成は任意であるが、医療用若しくは手術用のアームの場合は例えば６自由度以上の自由度を有している。図１では便宜上、手術アーム１２０に含まれる各リンクを、遠位端（若しくは、エンドエフェクター１１０の後端）から近い順に、第１のリンク、第２のリンク、…と呼ぶことにする。また、手術アーム１２０に含まれる各関節を、遠位端（若しくは、エンドエフェクター１１０の後端）から近い順に、第１の関節、第２の関節、…と呼ぶことにする。

【0021】

エンドエフェクター１１０は、手術アーム１２０の遠位端に、手首関節１１１を介して取り付けられているが、内視鏡の鏡筒や、超音波検査装置の探触子、メスや鉗子などの施術器具、気腹チューブ、エネルギー処置具、攝子、レトラクターといった各種の術具からなる。着脱式のエンドエフェクター１１０として構成して、さまざまな術具を交換して手術アーム１２０の遠位端に取り付けるようにしてもよい。内視鏡手術では、腹壁を切って開腹する代わりに、トロッカーと呼ばれる筒状の開孔器具が腹壁に複数穿刺され、エンドエフェクター１１０はトロッカーから患者の体腔内に挿入される（図示しない）。

【0022】

手術アーム装置１００は、図１に示すように手術室などの作業空間の床面に設置される場合の他に、図２に示すように、キャスターが装着された台車２００上に取り付けられて移動式となっている場合や、図３に示すように、手術台３００の側縁部３０１などに取り付けて用いられる場合など、さまざまな利用形態が想定される。

【0023】

例えば、手術台に手術アームを取り付けてそのアームの遠位端に硬性内視鏡を装着し、術者がヘッドマイクに向かって音声で内視鏡の動きを操作する装置（ＡＥＳＯＰ：Ａｕｔｏｍａｔｅｄ Ｅｎｄｏｓｃｏｐｉｃ Ｓｙｓｔｅｍ ｆｏｒ Ｏｐｔｉｍａｌ Ｐｏｓｉｔｉｏｎｉｎｇ）が知られている。

【0024】

さらに、手術台３００の患者支持面がヨー回りあるいはピッチ回りに回動可能な手術台が提供されている（例えば、特許文献２を参照のこと）。このような可動式の手術台３００に手術アーム装置１００が取り付けられた場合、図４や図５に示すように、手術台３００（又は、その患者支持面）と一体となって手術アーム装置１００も傾斜する。この結果、ローカル座標系上のＺ軸方向が手術台３００（又は、その患者支持面）の傾斜に伴って変化し、手術アーム１２０に作用する重力がローカル座標系上のＺ方向と一致するとは限らなくなる。

【0025】

一方、手術アーム１２０の各関節部の制御方法として位置制御と力制御を挙げることができる。本実施形態では、手術アーム装置１００が医療用途に使用され、対人物理インタラクションを実現するという観点から、手術アーム１２０の制御に力制御方式を採用するものとする。

【0026】

ここで、力制御方式においては、手術アームには姿勢に応じた重力（自重）による復元力が作用してしまう。そこで、手術アームに対して自重を打ち消すための機構を装備する、あるいは、手術アームの現在の姿勢で重力に打ち勝つために必要となる力を演算して指令信号に加えるといった「重力補償」を行なうのが一般的である。

【0027】

ところが、手術アーム装置１００を取り付けた手術台３００が傾斜したりした場合には、手術アーム１２０に対して重力方向とは異なる方向に重力補償機構が作用したり、重力方向とは異なる方向を用いて演算された力が指令信号に加わったりして、手術アーム１２０が期待した通りの動作を実行しなくなるおそれがある。重力方向とは異なる重力補償が手術アーム１２０に作用すると、オペレータは軽い操作感が得られない、手術アーム１２０がアンバランスとなり、想定していない方向を動き出すといった事態が発生する。とりわけ、医療用の手術アーム装置１００においては、手術アーム１２０のエンドエフェクター１１０は患者の体腔内に挿入して用いられるため（前述）、エンドエフェクター１１０の位置や発生力が指示値通りでないと事故を招来しかねない。

【0028】

そこで、本明細書では、手術アーム装置１００の傾斜に応じて正しい重力方向に基づいて重力補償のための力を演算して、適切な力制御を実現することができる制御システムについて提案する。

【0029】

図６には、力制御方式により手術アーム装置１００を制御する制御システム６００の機能的構成を模式的に示している。図示の制御システム６００は、状態取得部６０１と、演算条件設定部６０２と、力演算部６０３と、指令値変換部６０４を備えている。

【0030】

状態取得部６０１は、手術アーム装置１００の状態を取得する。一般的な力制御方式の制御システムでは、関節部で発生するトルクなどの力情報や、関節部の関節角度などの位置情報を、フィードバック情報として制御対象から取得する。本実施形態では、状態取得部６０１は、手術アーム１２０の力情報と位置情報に加えて、手術アーム装置１００本体の傾斜角度を取得するようになっている。

【0031】

演算条件設定部６０２は、状態取得部６０１で取得した情報に基づいて、手術アーム１２０の関節部に発生すべき力を算出する計算式（例えば、手術アーム１２０の運動方程式）の演算条件を設定して、同計算式を立式する。計算式には、重力補償項が含まれる。本実施形態では、状態取得部６０１から取得した手術アーム装置１００本体の現在の傾斜角度に応じて、正しい重力方向に対する重力補償項を演算条件として設定する。

【0032】

力演算部６０３は、演算条件設定部６０２で設定された演算条件に従って、上記計算式を解いて、手術アーム１２０の関節部に発生すべき力を算出する。演算条件設定部６０２で設定された演算条件には手術アーム装置１００本体の傾斜角度に応じた正しい重力方向に対する重力補償項が含まれている。したがって、本実施形態では、力演算部６０３は、手術アーム装置１００本体の傾斜角度に応じた正しい重力方向に対する適切な重力補償を行なって、関節部の発生力を算出することができる。なお、本実施形態では、手術アーム装置１００に設けられる複数の関節部の駆動を、後述する一般化逆動力学を用いた全身協調制御により制御することを想定している。

【0033】

指令値変換部６０４は、力演算６０３が算出した力情報を、手術アーム１２０側の関節部を制御するための指令値に変換して、手術アーム装置１００に出力する。なお、本実施形態では、関節部の駆動制御に関して、外乱の影響を補正することにより指令値に対する理想的な応答を実現する理想関節制御を適用することを想定している。

【0034】

手術アーム装置１００側では、駆動制御部６１３は、制御システム６００から受信した指令値に従って、手術アーム１２０を構成する関節部６１２の関節駆動部６１２－２（アクチュエータなど）の駆動を制御する。手術アーム１２０は複数の関節部及び複数のリンクから構成される多リンク構造体であるが、図６では簡素化のため単一の関節部６１２の構成のみを図示している。図示しない他の関節部も同様の構成であると理解されたい。関節部６１２は、手術アーム１２０において、リンク間を互いに回動可能に連結するとともに、駆動制御部６１３からの制御によりその回転駆動が制御されることにより、手術アーム１２０を駆動する。

【0035】

関節状態検出部６１２－１は、関節部６１２の回転角度や、関節部６１２の発生トルクといった関節部６１２の状態を検出して、制御システム６００側の状態取得部６０１に出力する。

【0036】

傾斜検出部６１１は、手術アーム装置１００若しくは手術アーム１２０の傾斜角を検出して、制御システム６００側の状態取得部６０１に出力する。図４や図５に示したように手術アーム装置１００が可動式の手術台に取り付けられている場合には、手術アーム装置１００の傾斜角は手術台とともに変化し、傾斜検出部６１１はその傾斜角を検出する。なお、手術アーム装置１００が傾斜検出部６１１を装備するのではなく、手術台３００が傾斜検出部６１１を備え、手術アーム装置１００と一体である手術台３００から、状態取得部６０１が患者支持面の傾斜角に関する情報を取得するようにしてもよい。

【0037】

続いて、本実施形態に係る手術アーム装置１００の全身協調制御に用いられる一般化逆動力学の概要について説明する。

【0038】

一般化逆動力学は、複数のリンクが複数の関節部によって連結されて構成される多リンク構造体（例えば、手術アーム１２０）において、各種の操作空間（Ｏｐｅｒａｔｉｏｎ Ｓｐａｃｅ）におけるさまざまな次元に関する運動目的を、各種の拘束条件を考慮しながら、複数の関節部に生じさせるトルクに変換する、多リンク構造体の全身協調制御における基本演算である。

【0039】

ここで言う操作空間は、手術アーム装置の力制御における重要な概念である。操作空間は、多リンク構造体に作用する力と多リンク構造体の加速度との関係を記述するための空間である。多リンク構造体の駆動制御を位置制御ではなく力制御によって行なう際に、多リンク構造体と環境との接し方を拘束条件として用いる場合に操作空間という概念が必要となる。操作空間は、例えば、多リンク構造体が属する空間である、関節空間、デカルト空間、運動量空間などである。

【0040】

また、ここで言う運動目的は、多リンク構造体の駆動制御における目標値を表し、例えば、駆動制御によって達成したい多リンク構造体の位置、速度、加速度、力、インピーダンスなどの目標値に相当する。

【0041】

また、ここで言う拘束条件は、多リンク構造体の形状や構造、多リンク構造体の周囲の環境及びユーザーによる設定などによって定められる、多リンク構造体の位置、速度、加速度、力などに関する拘束条件である。例えば、拘束条件には、発生力、優先度、非駆動関節の有無、垂直反力、摩擦錘、支持多角形などについての情報が含まれる。

【0042】

一般化動力学においては、数値計算上の安定性と実時間処理可能な演算効率とを両立するため、その演算アルゴリズムは、第１段階である仮想力算出処理と、第２段階である実在力算出処理で構成される。第１段階の仮想力算出処理では、各運動目的の達成に必要な、操作空間に作用する仮想的な力である仮想力を、運動目的の優先度と仮想力の最大値を考慮しながら決定する。続く第２段階の実在力算出処理では、非駆動関節、垂直反力、摩擦錘、支持多角形などに関する拘束を考慮しながら、上記で得られた仮想力を関節力、外力などの実際の多リンク構造体の構成で実現可能な実在力に変換する。

【0043】

ここで、第１段階の仮想力算出処理について説明する。

【0044】

多リンク構造体の各関節部におけるある物理量によって構成されるベクトルを一般化変数ｑと呼ぶ（関節値ｑ又は関節空間ｑとも呼ぶ）。操作空間ｘは、一般化変数ｑの時間微分値とヤコビアンＪを用いて、下式（１）で定義される。

【0045】

【数1】

【0046】

本実施形態に係る手術アーム装置１００では、例えば、ｑは手術アーム１２０を構成する各関節部における回転角度に相当する。手術アーム１２０のようなリンク構造物全体の操作空間ｘに関する運動方程式は、下式（２）に示すような形で表現することができる。

【0047】

【数2】

【0048】

上式（２）において、τは一般化変数ｑに対応した一般化力、ｂは重力、コリオリ力、及び遠心力、ｆは操作空間に作用する外力である。また、Ｈは構造全体の関節空間に対する慣性行列、すなわち関節空間慣性行列である。上式（２）を下式（３）のように変形する。

【0049】

【数3】

【0050】

上式（３）において、Λ^-1は、操作空間慣性逆行列と呼ばれる。操作空間慣性逆行列Λ^-1は、ヤコビアンＪと関節空間慣性行列Ｈを用いて、下式（４）のように表される。

【0051】

【数4】

【0052】

また、上式（３）の右辺第３項のｃは、操作空間バイアス加速度、すなわち外力が作用しない場合に操作空間に作用する加速度に相当する。操作空間バイアス加速度は、下式（５）のように表される。

【0053】

【数5】

【0054】

上式（５）中のｂは、重力、コリオリ力、及び遠心力に相当する（前述）。本実施形態では、手術アーム装置１００から可動式の手術台３００に取り付けられ、傾斜角が動的に変化すること（図４、図５を参照のこと）を想定している。したがって、重力ｇと、コリオリ力及び遠心力ｖは、関節角ｑ又は関節角速度の関数であり、ｂを下式（６）のように表す。また、操作空間バイアス加速度ｃを、下式（７）のように書き換えることができる。図６に示した制御システム６００の構成によれば、傾斜検出部６１１によって検出される関節角ｑを、状態取得部６０１が取得するようになっている。

【0055】

【数6】

【0056】

【数7】

【0057】

一般化逆動力学においては、操作空間ｘに関する位置、速度の運動目的は、操作空間ｘの加速度として表現できることが知られている。このとき、上式（１）から、運動目的として与えられた目標値である操作空間加速度を実現するために、操作空間ｘに作用するべき仮想力ｆ_vは、下式（８）のような一種の線形相補性問題（Ｌｉｎｅａｒ Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｐｒｏｂｌｅｍ）を解くことによって得られる。

【0058】

【数8】

【0059】

上式（８）の左辺第２項として、上式（７）に示したように、状態取得部６０１から取得した手術アーム装置１００本体の現在の傾斜角度に応じて、正しい重力方向に対する重力補償項を含んだ操作空間バイアス加速度ｃを用いるようにする。その結果、上式（８）を解くことによって、正しい重力方向に対する適切な重力補償を施した仮想力ｆ_vを求めることができる、という点を理解されたい。

【0060】

ここで、上式（８）において、Ｌ_iとＵ_iはそれぞれ、ｆ_vの第ｉ成分の負の下限値（－∞を含む）、ｆ_vの第ｉ成分の正の上限値（＋∞を含む）とする。線形相補性問題は、例えばＩｔｅｒａｔｉｖｅ法、Ｐｉｖｏｔ法、ロバスト加速度制御を応用する方法などを用いて解くことができる。

【0061】

なお、操作空間慣性逆行列Λ^-1とバイアス加速度ｃは、定義式である上式（４）、（５）の通り算出すると計算コストが大きい。そこで、多リンク構造体の一般化力（関節力τ）から一般化加速度（関節加速度）を得る順動力学計算（Ｆｏｒｗａｒｄ Ｄｙｎａｍｉｃｓ：ＦＷＤ）を応用することにより、操作空間慣性逆行列Λ^-1の算出処理をより高速に算出する方法が提案されている。具体的には、順動力学演算を用いることにより、関節空間ｑ、関節力τ、重力ｇなどの多リンク構造体に作用する力に関する情報から、操作空間慣性逆行列Λ^-1とバイアス加速度ｃを算出することができる。このように、操作空間に関する順動力学演算を応用することにより、関節部の数Ｎに対してＯ（Ｎ）の計算量で操作空間慣性逆行列Λ^-1を算出することができる。

【0062】

図６に示した制御システム６００では、操作空間慣性逆行列Λ^-1の算出に際し、多リンク構造体としての手術アーム装置１００に作用する力に関する情報（関節空間ｑ、関節力τ、重力ｇ）を、関節状態検出部６１２－１と、傾斜検出部６１１から取得するように構成されている。傾斜検出部６１１から傾斜角に基づいて、手術アーム装置１００が取り付けられた手術台３００の傾斜角に応じて、正しい重力方向の情報を得ることができるという点を十分理解されたい。

【0063】

運動目的の設定例として、絶対値Ｆ_i以下の仮想力ｆ_viで操作空間加速度の目標値（ｘの２階微分に上付きバーを付して表す）を達成するための条件を、下式（９）で表すことができる。

【0064】

【数9】

【0065】

また、上述したように、操作空間ｘの位置、速度に関する運動目的は、操作空間加速度の目標値として表すことができ、具体的には下式（１０）で表すことができる（但し、操作空間ｘの位置、速度の目標値を、ｘ、ｘの１階微分に上付きバーを付して表す）。

【0066】

【数10】

【0067】

その他、分解操作空間の考え方を用いることにより、他の操作空間の線形和で表される操作空間（運動量、デカルト相対座標、連動関節など）に関する運動目的を設定することもできる。なお、競合する運動目的間には、優先度を与える必要がある。優先度毎且つ低優先度から順に上記線形相補性問題を解いて、前段の線形相補性問題で得られた仮想力を次段の線形相補性問題の既知外力として作用させることができる。

【0068】

続いて、第２段階の実在力算出処理について説明する。

【0069】

上記のようにして得られた仮想力ｆ_vを、実在の関節力と外力に変換する処理を行なう。仮想力による一般化力τ_v（＝Ｊ_v ^Tｆ_v）を関節部に生じる発生トルクτ_aと外力ｆ_eとで実現するための条件は、下式（１１）のように表される。

【0070】

【数11】

【0071】

上式中の添え字ａは駆動関節部の集合（駆動関節集合）を表し、添え字ｕは非駆動関節部の集合（非駆動関節集合）を表す。すなわち、上式（１１）の上段は非駆動関節部による空間（非駆動関節空間）の力の釣り合いを表し、下段は駆動関節部による空間（駆動関節空間）の力の釣合いを表している。Ｊ_vu、Ｊ_vaは、それぞれ、仮想力ｆ_vが作用する操作空間に関するヤコビアンの非駆動関節成分、駆動関節成分である。Ｊ_eu、Ｊ_eaは、外力ｆ_eが作用する操作空間に関するヤコビアンの非駆動関節成分、駆動関節成分である。また、Δｆ_vは仮想力ｆ_vのうち、実在力で実現不能な成分を表す。

【0072】

上式（１１）の上段は不定であり、例えば下式（１２）に示すような２次計画問題（Ｑｕａｄｒａｔｉｃ Ｐｒｏｇｒａｍｉｎｇ Ｐｒｏｂｌｅｍ）を解くことで、ｆ_e及びΔｆ_vを得ることができる。

【0073】

【数12】

【0074】

上式（１２）中、εは上式（１１）の上段り、変数ベクトルを表す。Ｑ１及びＱ２は、最小化の際の重みを表す正定値対称行列である。また、上式（１２）の不等式拘束は、垂直反力、摩擦錐、外力の最大値、支持多角形などの外力に関する拘束条件を表すのに用いられる。例えば、矩形の支持多角形に関する不等式拘束は、下式（１３）のように表現される。

【0075】

【数13】

【0076】

ここで、ｚは接触面の法線方向を表し、ｘ及びｙはｚに垂直な直交２接線方向を表す。（Ｆ_x，Ｆ_y，Ｆ_z）及び（Ｍ_x，Ｍ_y，Ｍ_z）は、接触点に作用する外力及び外力モーメントである。μ_t及びμ_rは、それぞれ並進、回転に関する摩擦係数である。（ｄ_x，ｄ_y）は支持多角形のサイズを表している。

【0077】

上式（１２）、（１３）から、最小ノルム又は最小誤差の解ｆ_e、Δｆ_vが求められる。上式（１２）から得られたｆ_e、Δｆ_vを上式（１１）の下段に代入することにより、運動目的を実現するために必要な関節力τ_aを得ることができる。

【0078】

基底が固定され、非駆動関節が無い系の場合は、関節力のみで全ての仮想力を置換可能であり、上式（１１）において、ｆ_e＝０、Δｆ_v＝０とすることができる。この場合、上式（１１）の下段から、下式（１４）に従って実在の関節力τ_aを得ることができる。

【0079】

【数14】

【0080】

上記のように、一般化逆動力学を用いて仮想力算出処理及び実在力算出処理を順に行なうことにより、所望の運動目的を達成するための関節力τ_aを得ることができる。逆に言えば、一般化逆動力学を用いて算出された関節力τ_aを手術アーム装置１００の関節部６１２の運動における理論モデルに反映することにより、関節部６１２が力制御方式により所望の運動目的を達成するように駆動される。

【0081】

また、本実施形態では、関節力τ_aを算出する過程（より厳密には、仮想力を算出する過程）において、傾斜検出部６１１で検出される手術アーム装置１００本体の傾斜角度に基づいて正しい重力方向に対する適切な重力補償を行なって、関節部の発生力を算出することができる。

【0082】

なお、ここまで説明した一般化逆動力学を用いた全身協調制御、特に、仮想力ｆ_vの導出過程や、上記の線形相補性問題を解いて仮想力ｆ_vを求解する方法、２次計画問題の解法などの詳細については、例えば、本出願人に既に譲渡されている特開２００９－９５９５９号公報や特開２０１０－１８８４７１号公報を参照されたい。

【0083】

続いて、本実施形態における関節部の駆動制御方法に適用される、理想関節制御について説明する。

【0084】

上記の一般化逆動力学を用いた演算により求められた関節力指令値τ_aに従って、関節部６１２を駆動するアクチュエータを、力制御方式により制御する。

【0085】

図７には、関節部６１２を駆動するアクチュエータ７００の構成例を示している。図示のアクチュエータ７００は、回転トルクを発生するためのモータ（例えば、ＤＣブラシレス・モータ）７０１と、モータ７０１の回転力を十分な発生力に変換するための減速機７０２で構成される。また、減速機７０２の出力軸には、関節角を計測するためのエンコーダ７０３と、回転トルクを検出するトルク・センサ７０４が取り付けられている。エンコーダ７０３とトルク・センサ７０４は、図６中の関節状態検出部６１２－１に相当する。なお、トルク・センサ７０４として、例えば本出願人に既に譲渡されている特開２００９－２８８１９８号公報に開示されているトルク計測装置を適用することができる。

【0086】

上記の一般化逆動力学を用いた演算により算出した実在力τ_aを関節力指令値に用いて、アクチュエータ７００に対する力制御を実施することにより、外乱の影響を回避することができる。ところが、関節アクチュエータがダイナミクス演算で用いられる理論モデルから逸脱した応答を行なうと、ダイナミクス演算により算出されたトルクをアクチュエータに作用させても、目的の運動状態は達成されない。

【0087】

実際、図７に示したような減速機７０２を有したアクチュエータ７００には、摩擦や慣性などの、モデル化や同定が困難となるダイナミクスのパラメータが含まれており、力制御に影響を与える外乱の主要因となる。かかる外乱の問題に対し対策を施さなければ、容易に理論モデルから逸脱することになる。すなわち、誤差の主要因となる関節部の摩擦並びに慣性が理論モデルと合致するよう、理想的なアクチュエータを関節に用いることがさらに必要である。

【0088】

そこで、本実施形態では、摩擦や慣性といったモデル化できない外乱の影響を受けつつも、理論モデル通りの応答を行なうようにアクチュエータ７００の応答を修正する方法を適用して、関節部６１２を理想関節として駆動制御するようにする。以下では、関節部６１２を駆動するアクチュエータ７００が理論モデルに従った応答を行なうための理想関節制御の方法について説明する。

【0089】

手術アーム装置１００（若しくは、手術アーム１２０）のダイナミクス演算において、アクチュエータ７００は、下式（１５）に示すような２次遅れ系の運動方程式によってモデル化される。

【0090】

【数15】

【0091】

上式（１５）において、Ｉ_aは関節の慣性モーメント（仮想イナーシャ）、ｑは（エンコーダ７０４の出力として得られる）関節の関節角、τ_aは関節の発生トルクの指令値、τ_eは関節に作用する外トルク、ν_aは関節内部の（未知の、モデル化が困難となる）仮想的な粘性係数である。上式（１５）は、アクチュエータ７００の運動を表す理論モデルと言うこともできる。

【0092】

上述した一般化逆動力学を用いた演算により、アクチュエータ７００に作用させるべき実在力τ_aを、運動目的及び拘束条件に応じて算出することができる。そして、理想的には、アクチュエータ７００は上式（１５）に示す理論モデルに従った応答が実現して、所望の運動目的が達成されるはずである。

【0093】

しかしながら、実際には、摩擦や慣性といったモデル化できない外乱の影響により、アクチュエータ７００の運動と上式（１５）に示す理論モデルとの間には誤差、すなわちモデル化誤差が生じる場合がある。モデル化誤差は、多リンク構造体の重量、重心、慣性テンソルなどのマスプロパティに起因するものと、多リンク構造体におけるアクチュエータ７００内部における摩擦や慣性などに起因するものとに大別することができる。

【0094】

このうち、前者のマスプロパティに起因するモデル化誤差は、ＣＡＤ（Ｃｏｍｐｕｔｅｒ Ａｉｄｅｄ Ｄｅｓｉｇｎ）データの高精度化や同定手法の適用によって、理論モデル構築時に比較的容易に低減することが可能である。一方、後者のアクチュエータ７００内部の摩擦や慣性などに起因するモデル化誤差は、例えばアクチュエータ７００の減速機７０２における摩擦など、モデル化が困難な現象に起因しており、理論モデル構築時に無視できないモデル化誤差が残留し得る。

【0095】

また、上記の運動方程式（１５）におけるイナーシャＩ_aや粘性抵抗係数ν_eの値と、実際のアクチュエータ７００におけるこれらの値との間に誤差が生じている可能性がある。これらのモデル化が困難な誤差は、アクチュエータ７００の駆動制御において外乱となり得る。

【0096】

したがって、このような外乱の影響により、実際には、アクチュエータ７００は、上式（１５）に示す理論モデル通りには応答しない場合がある。その結果、一般化逆動力学によって算出された関節力としての実在力τ_aをアクチュエータ７００に適用しても、制御目標である運動目的が達成されなくなる。

【0097】

そこで、本実施形態では、アクチュエータ７００が上式（１５）に示す理論モデルに従った理想応答を行なうように、アクチュエータ７００にアクティブな制御系を付加して応答を修正する方法を考える。具体的には、アクチュエータ７００のトルク・センサ７０４を用いた摩擦補償型のトルク制御を行なうに留まらず、要求される発生トルクτ_a及び外トルクτ_eに対して、イナーシャＩ_a及び粘性抵抗係数ν_aに至るまで理論値に従った理想応答を行なうことを可能にする。

【0098】

図８には、アクチュエータ７００が理論モデルに従った応答を行なうための制御ブロック図を示している。図示の制御ブロックは、例えば、図６に示した制御システム６００内で、指令値変換部６０４が、力演算部６０３で算出された実在力τ_aを、理想応答を行なうために修正する処理に相当する。

【0099】

図８中、点線で囲まれた部分は外乱オブザーバ８１０に相当し、アクチュエータ７００に対する外乱トルクτ_dを推定し、当該制御系に及ぼす影響を除去することによって、ロバストな加速と瀬制御系を構築している。但し、Ｊ_nは関節内のイナーシャの公称値、Ｊは関節内のイナーシャの（未知の）実際値、ｑは関節角とする。また、関節の仮想イナーシャＩ_aは、ダイナミクス演算における設計事項として、仮想的な定数が与えられるものとする。

【0100】

力演算部６０３では、制御周期毎に、上述した一般化逆動力学を用いた演算により、アクチュエータ７００へのトルク指令値τ_aを決定する。また、アクチュエータ７００の減速機７０２に出力軸に取り付けられたトルク・センサ７０４によって計測された外トルク実測値τ_e、並びに減速機７０２の出力軸に取り付けられたエンコーダ７０３によって計測された関節角ｑから得られる角速度実測値が、関節状態検出部６１２－１から指令値変換部６０４に送られてくる。

【0101】

参照番号８０１は、上式（１５）に示す運動方程式で表される理想応答モデルに従った演算を行なう演算器である。演算器８０１は、トルク指令値τ_a、外トルク実測値τ_e、及び関節角速度の実測値（関節角ｑの１階微分）を入力して、上式（１５）の左辺の関節角ｑの加速度（関節角目標値ｑ^refの２階微分）を算出して、これを関節角加速度目標値として外乱オブザーバ８１０に入力する。

【0102】

参照番号８０２は、アクチュエータ７００の回転角加速度に基づいてアクチュエータ７００に発生するトルクを算出する演算器である。この演算器８０２は、外乱オブザーバ８１０に入力された関節角ｑの加速度目標値に関節の仮想イナーシャ公称値Ｊ_nを掛けて、現制御周期におけるトルク目標値τ^refに変換する。

【0103】

理想の応答においては、アクチュエータ７００にトルク目標値τ^refを生じさせることにより、所望の運動目的を達成できるはずである。しかしながら、上述したように、実際の応答には外乱などの影響が生じる場合がある。そこで、本実施形態では、制御ブロックが外乱トルクτ_dを推定する外乱オブザーバ８１０を装備する構成とした。したがって、前制御周期で外乱オブザーバ８１０により推定された外乱トルクτ_dで現制御周期のトルク目標値τ^refに修正を加えるというフィードフォワード制御を行なうことで、制御ブロックは、現制御周期における関節に対するトルク指令値τを得ることができる。

【0104】

参照番号８０３は、関節であり、伝達関数１／Ｊ_nからなる。この関節８０３にトルク指令値τからなる力制御を掛けると、摩擦や慣性などの外難の影響を受けながら回転駆動する。具体的には、指令値変換部６０４がトルク指令値τを電流指令値に換算して、これが駆動制御部６１３（若しくは、モータ７０１を駆動するモータ・ドライバ）への指示入力となる。

【0105】

関節８０３の駆動の際に関節に発生する外トルクτ_e並びに関節角ｑはそれぞれトルク・センサ７０４並びにエンコーダ７０３で計測される。また、参照番号８０４は、微分演算を行なう演算器であり、エンコーダ７０３で計測される関節角ｑを時間微分して関節角速度を得る。ここで得られた関節角速度は、理想応答モデルに従った演算を行なう演算器８０１（前述）への入力となる。

【0106】

外乱オブザーバ８１０は、それぞれ参照番号８１１、８１２で示す演算器を含んでいる。演算器８１１は、演算器８０４から出力される関節角速度に対して、関節の仮想イナーシャ公称値Ｊ_nからなる伝達関数Ｊ_nｓを適用することで、関節に作用したトルクを推定する。そして、演算器８１１による推定トルクをアクチュエータ７００への現制御周期のトルク指令値τから引き算することで、外乱トルクτ_dを推定することができる。そして、現制御周期で得られた外乱トルクを次制御周期におけるトルク目標値τ^refに修正を加えるというフィードフォワード制御を行なうことで、次制御周期における関節に対するトルク指令値τを得ることができる。

【0107】

また、演算器８１２は、系の発散を防ぐために、外乱トルクτ_dの推定値を出力する途中に挿入されたローパス・フィルター（ＬＰＦ： Ｌｏｗ Ｐａｓｓ Ｆｉｌｔｅｒ）である。演算器８１２は、伝達関数ｇ／（ｓ＋ｇ）で表される演算を行なうことにより、入力値に対して低周波成分のみを出力して、系を安定化させることができる。

【0108】

このようにして、関節部に摩擦や慣性などのモデル化することができない外乱成分が存在していても、アクチュエータ７００の加速度応答を加速度目標値に追従させることができる。すなわち、上式（１５）の右辺が与えられたときに左辺の関節角加速度が達成されるので、アクチュエータ７００は外乱の影響を受けるにも拘らず理論モデルに従った応答を実現することができる。なお、外乱トルクτ_dを出力する途中に上記のローパス・フィルターｇ／（ｓ＋ｇ）８１２が挿入されており（前述）、高周波数域の外乱除去には向かない。

【0109】

このように図８に示した制御ブロック構成によれば、アクチュエータ７００の駆動制御において、摩擦や慣性などの外乱成分があった場合であっても、アクチュエータ７００の応答を目標値に追従させることが可能となる。また、関節部６１２の駆動制御について、理論モデルが仮定するイナーシャＩ_a及び粘性抵抗係数ν_aに従った理想応答を行なうことが可能となる。

【0110】

なお、理想関節制御の詳細については、例えば本出願人に既に譲渡されている特開２００９－２６９１０２号公報を参照されたい。

【0111】

図９には、一般化逆動力学演算を利用して力制御を実現する手術アーム装置１００の制御システム９００の機能的構成を示している。

【0112】

制御システム９００の制御対象である手術アーム装置１００は、手術アーム１２０のエンドエフェクター１１０としての撮像部と、傾斜検出部９１１と、手術アーム１２０と、手術アーム１２０の関節部９１２の駆動を制御する駆動制御部９１３を備えている。

【0113】

エンドエフェクター１１０として取り付けられる撮像部は、例えば手術時にトロッカーから患者の体腔内に挿入して用いられる硬性内視鏡である。具体的には、撮像部は、撮影対象を動画や静止画の形式で撮影することのできるカメラなどであり、２次元上に配列された複数の受光素子を有し、この受光素子における光電変換により撮影対象である患者の体腔内の画像を表す画像信号を取得することができる。

【0114】

この撮像部による撮像画像を表示する表示装置９２０を手術アーム装置１００に外付け接続し、又は手術アーム装置１００が装備していてもよい。表示装置９２０は、各種の情報を表示画面上にテキストやイメージなどさまざまな形式で表示するデバイスであり、画像信号に対して各種の画像処理を施す画像信号処理部や処理された画像信号に基づく画像を画面上に表示出力する表示駆動部などを備えている。本実施形態では、表示装置９２０は、手術アーム１２０のエンドエフェクター１１０である撮像部が撮影した画像を画面上に表示する。

【0115】

傾斜検出部９１１は、手術アーム装置１００若しくは手術アーム１２０の傾斜角を検出する。例えば、図４や図５に示したように手術アーム装置１００が可動式の手術台に取り付けられている場合には、手術アーム装置１００の傾斜角は手術台とともに動的に変化し、傾斜検出部９１１はその傾斜角を検出する。なお、手術アーム装置１００が傾斜検出部９１１を装備するのではなく、手術台３００が傾斜検出部９１１を備え、手術アーム装置１００と一体である手術台３００から、患者支持面の傾斜角に関する情報を取得するようにしてもよい。傾斜検出部９１１は、検出した傾斜角を、制御システム９００側に送信する。

【0116】

ローカル座標系上のＺ軸方向が手術台３００（又は、その患者支持面）の傾斜に伴って変化するため、手術アーム１２０に作用する重力がローカル座標系上のＺ方向と一致するとは限らなくなる。これに対し、本実施形態では、手術アーム１２０に作用する重力の方向をＺ方向に固定するのではなく、傾斜検出部９１１により検出された傾斜角の関数として扱うことが可能になる。

【0117】

手術アーム１２０は、複数の関節部及び複数のリンクから構成される多リンク構造体であり、関節部の駆動を制御する駆動制御部９１３を備えている。手術アーム１２０は複数の関節部及び複数のリンクから構成されるが、図９では簡素化のため単一の関節部９１２の構成のみを図示している。図示しない関節部も同様の構成であると理解されたい。すなわち、駆動制御部９１３によって駆動が制御されることによって、複数の関節部がそれぞれの可動範囲内で駆動することにより、エンドエフェクター１１０としての撮像部の位置及び姿勢を制御することができる。

【0118】

駆動制御部９１３は、例えば、関節部９１２の駆動用のアクチュエータのモータに対して供給する電流量を制御することにより、モータの回転数を制御して、関節部９１２の回転角度及び発生トルクを制御する。但し、駆動制御部９１３による関節部９１２の駆動制御は、制御システム９００による演算結果に基づいて行なわれる。したがって、駆動制御部９１３が関節部９１２のアクチュエータ・モータに対して供給する電流量は、基本的には制御システム９００の制御下で決定される。

【0119】

関節部９１２は、手術アーム１２０において、リンク間を互いに回動可能に連結するとともに、駆動制御部９１３からの制御によりその回転駆動が制御されることにより、手術アーム１２０を駆動する。関節部９１２は、関節駆動部９１２－２と、関節状態検出部９１２－１を備えている。

【0120】

関節駆動部９１２－２は、関節部９１２の駆動機構であり、例えば図７に示したアクチュエータのモータからなる。駆動制御部９１３により駆動が制御されて、関節駆動部９１２－２が駆動することにより、関節部９１２が回転駆動する。

【0121】

関節状態検出部９１２－１は、関節部９１２の回転角度や、関節部９１２の発生トルクといった関節部９１２の状態を検出する。関節状態検出部９１２－１は、図７に示したアクチュエータ７００の減速機７０２の出力軸に取り付けられた、エンコーダ７０３並びにトルク・センサ７０４からなる。関節状態検出部９１２－１は、検出した関節部９１２の状態を制御システム９００側に送信する。

【0122】

一方、制御システム９００は、全身協調制御部９３０と、理想関節制御部９４０と、入力部９５０と、記憶部９６０を備えている。制御システム９００は、例えば、ＰＣ（Ｐｅｒｓｏｎａｌ Ｃｏｍｐｕｔｅｒ）やサーバなどの、制御対象である手術アーム装置１００に外部接続される各種の情報処理装置によって構成される。あるいは、制御システム９００は、制御対象である手術アーム装置１００上に搭載されるコントローラなどの装置であってもよい。

【0123】

入力部９５０は、ユーザーが制御システム９００に対して手術アーム装置１００の駆動制御に関する情報や命令などを入力するための入力インターフェースである。具体的には、入力部９５０は、マウスやキーボード、タッチパネル、ボタン、スイッチ、レバー、ジョイスティック、ペダルなどの操作デバイスを備えている。例えば、ユーザーの入力部９５０に対する操作入力に応じて、手術アーム１２０の駆動が制御され、エンドエフェクター１１０である撮像部の位置や姿勢が制御される。

【0124】

記憶部９６０は、制御システム９００において処理される各種の情報を記憶する。本実施形態では、記憶部９６０は、制御システム９００において実施される手術アーム装置１００の全身協調制御や、関節部の理想関節制御に関する演算において用いられる各種のパラメータを記憶することができる。例えば、記憶部９６０は、演算条件設定部９３２が全身協調制御に関する演算に用いられる運動目的や拘束条件に関する情報（後述）を記憶しておいてもよい。また、本実施形態では、記憶部９６０は、手術アーム１２０に関する各種のアーム情報（例えば、手術アーム１２０を構成する関節及びリンクの数、リンクと関節の接続状況、リンクの長さなどの情報）を記憶することができる。

【0125】

全身協調制御部９３０は、手術アーム１２０の運動目的及び拘束条件に基づいて、操作空間における関節部９１２に対するトルク指令値τ_aを、一般化逆動力学を用いた演算により算出する機能ブロックである。全身協調制御部９３０は、関節状態取得部９３１と、演算条件設定部９３２と、仮想力推定部９３３と、実在力算出部９３４を備えている。

【0126】

状態取得部９３１は、関節状態検出部９１２－１で検出される関節部９１２の回転角度及び関節９１２に作用する外トルクを取得することができる。また、記憶部９６０は、制御システム９００によって処理される各種の情報を記憶しているが、具体的には、手術アーム１２０に関する各種のアーム情報（例えば、手術アーム１２０を構成する関節及びリンクの数、リンクと関節の接続状況、リンクの長さなどの情報）が記憶されている（前述）。状態取得部９３１は、記憶部９６０からこのようなアーム情報を取得することができる。したがって、状態取得部９３１は、関節部９１２の状態とアーム情報とに基づいて、複数の関節、複数のリンク、及び撮像部の空間上の位置（座標）（すなわち、手術アーム１２０の形状や撮像部の位置及び姿勢）、各関節、リンク及び撮像部に作用している力などの情報をアーム状態として取得することができる。状態取得部９３１は、取得したアーム情報を演算条件設定部９３２に送信する。また、状態取得部９３１は、傾斜検出部９１１で検出される手術アーム装置１００若しくは手術アーム１２０の傾斜角を取得することができる。

【0127】

演算条件設定部９３２は、一般化逆動力学を用いた全身協調制御に関する演算における演算条件を設定する。ここで言う演算条件は、例えば運動目的及び拘束条件である。運動目的は、手術アーム１２０の運動に関する各種の情報であり、具体的には、撮像部の位置及び姿勢（座標）、速度、加速度、並びに力などの目標値や、手術アーム１２０を構成する複数の関節部及び複数のリンクの位置（座標）、速度、加速度及び力などの目標値である。また、ここで言う拘束条件は、手術アーム１２０の運動を制限又は拘束する各種の情報であり、具体的には、手術アーム１２０の各構成部材が移動不可能な領域の座標や、移動不可能な速度、加速度の値、発生不可能な力の値などである。

【0128】

また、演算条件設定部９３２は、状態取得部９３１で取得される手術アーム装置１００若しくは手術アーム１２０の傾斜角に応じた重力、コリオリ力、及び遠心力ｂを、上式（６）に従って設定する。

【0129】

また、演算条件設定部９３２は、手術アーム１２０の構造的に実現することが不可能となる各種物理量の制限範囲を拘束条件として設定するようにしてもよい。また、演算条件設定部９３２は、手術アーム１２０の構造についての物理モデル（例えば、手術アーム１２０を構成するリンクの数や長さ、関節部を介したリンクの接続状況、関節部の可動範囲などがモデル化されたもの）を有し、当該物理モデルに、所望の運動条件及び拘束条件が反映された制御モデルを生成することにより、運動条件及び拘束条件を設定してもよい。

【0130】

本実施形態では、運動目的及び拘束条件を適切に設定することにより、手術アーム１２０に所望の動作を行なわせることが可能となる。例えば、運動目的として、エンドエフェクター１１０としての撮像部の位置の目標値を設定することにより撮像部をその目標の位置に移動させることはもちろんのこと、手術・アーム１２０が空間上の所定の領域内に侵入しない（例えば、患者の体腔を侵襲しない）ようにするなどの拘束条件によって移動の制約を設けて手術アーム１２０を駆動させることも可能である。

【0131】

運動目的の具体例として、エンドエフェクター１１０としての撮像部の撮影方向が施術部位に固定された状態で、撮像部が特定の施術部位を頂点とした円錐の面内を、当該円錐の軸を旋回軸として旋回するピボット動作を挙げることができる。ピボット動作では、例えば、撮像部と円錐の頂点に相当する点との距離が一定に保たれた状態で旋回動作が行なわれる。このような撮像部のピボット動作を行なうことにより、観察部位を等距離から且つ異なる角度から観察することができるので、施術者の利便性を向上させることができる。

【0132】

また、他の具体例として、手術アーム１２０を構成する各関節部における発生トルクを制御する運動目的であってもよい。具体的には、アーム部１２０に作用する重力を打ち消すように関節部１３０の状態を制御し、さらに外部から与えられた力の方向へのアーム部１２０の移動をサポートするように関節部の状態を制御するパワー・アシスト動作を運動目的とすることもできる。パワー・アシスト動作においては、手術アーム１２０を構成する各関節部における重力による外トルクを打ち消す発生トルクを生じさせるように各関節部の駆動が制御されることにより、手術アーム１２０の位置及び姿勢が所定の状態で保持される。この状態でさらに外部から（例えばユーザーから）外トルクが加えられた場合に、与えられた外トルクと同じ方向の発生トルクを生じさせるように各関節部の駆動が制御される。このようなパワー・アシスト動作を行なうことにより、ユーザーが手動で手術アーム１２０を動かす場合に、ユーザーはより小さい力で手術アーム１２０を移動させることができ、あたかも無重力下で手術アーム１２０を動かしているような感覚をユーザーに対して与えることができる。また、上述したピボット動作とパワー・アシスト動作とを組み合わせることも可能である。

【0133】

本実施形態における運動目的は、瞬時的な運動目的（例えばある時間における手術アーム１２０の各構成部材の位置や速度、力などの目標値）と、瞬時的な運動目的が連続的に達成された結果として経時的に実現される手術アーム１２０の各構成部材の動作の双方を含む概念である。例えば、上記のピボット動作において、撮像部がピボット動作を行なうこと自体が運動目的であるが、ピボット動作の最中において円錐面内での撮像部の位置や速度などの値が瞬時的な運動目的（当該運動目的における目標値）として設定されている。また、上記のパワー・アシスト動作において、外部から加えられた力の方向への手術アーム１２０の移動をサポートするパワー・アシスト動作を行なうこと自体が運動目的であるが、パワー・アシスト動作の最中において各関節部に加えられる外トルクと同じ方向への発生トルクの値が瞬時的な運動目的（当該運動目的における目標値）として設定されている。全身協調制御部２４０における全身協調制御のための演算における各ステップでは瞬時的な運動目的がその都度設定され、当該演算が繰り返し行なわれることにより、最終的に所望の運動目的が達成される。

【0134】

なお、演算条件設定部９３２において運動目的を設定する際に、各関節部の回転運動における粘性抵抗係数も併せて適宜設定するようにしてもよい（但し、関節駆動用のアクチュエータが回転運動における粘性抵抗係数を調整できるように構成されている場合）。例えば、設定する運動目的に応じて、各関節部を、外部から加えられる力に対して回転し易い状態や逆に回転し難い状態に設定することができる。具体的には、上述したパワー・アシスト動作において、各関節部における粘性抵抗係数を小さく設定することにより、ユーザーはより小さな力で手術アーム１２０を移動させることができ、ユーザーに対して無重力感を与えることができる。

【0135】

また、演算条件設定部９３２は、状態取得部９３１が取得する手術アーム１２０の状態に基づいて運動目的を設定するようにしてもよい。例えば、ユーザーが手動で手術アーム１２０を移動させようとする場合の、手術アーム１２０の位置情報や手術アーム１２０に作用する力の情報を状態取得部９３１で取得し、演算条件設定部９３２は、ユーザーが手術アーム１２０を移動させた位置や速度、力などを瞬間的な運動目的として設定することができる。このような運動目的の設定方法によれば、ユーザーによる手術アーム１２０の移動を追随し、サポートするように手術アーム１２０の駆動を制御することができる。

【0136】

また、演算条件設定部９３２は、入力部９５０を介したユーザーからの指示に従って運動目的や拘束条件を設定するようにしてもよい。例えば、入力部９５０として装備されるレバーやペダルなどの操作デバイスに対してユーザーが行なった操作に応じて、演算条件設定部９３２は、手術アーム１２０の各構成部材の位置や速度などの瞬間的な運動目的を設定するようにしてもよい。

【0137】

また、記憶部９６０に運動目的並びに拘束条件などの演算条件が記憶されている場合には、演算条件設定部９３２は、記憶部９６０から読み出した演算条件を設定することができる。例えば、エンドエフェクター１１０としての撮像部が空間上の所定の位置で静止するという運動目的であれば、その所定の位置の座標を運動目的として記憶部９６０にあらかじめ記憶しておくことができる。また、例えば、撮像部が空間上で所定の軌跡上を移動するという運動目的であれば、その軌道を補間できる複数の点の座標を運動目的として記憶部９６０にあらかじめ記憶しておくことができる。また、撮像部のピボット動作（前述）が運動目的であれば、撮像部が旋回する円錐の面内における位置や速度などの目標値を記憶部９６０にあらかじめ記憶しておくことができる。また、パワー・アシスト動作が運動目的であれば、力の目標値を記憶部９６０にあらかじめ記憶しておくことができる。なお、記憶部９６０に記憶されている拘束条件の中に運動目的の優先度を設定することも可能である。複数の互いに異なる運動目的が存在する場合には、演算条件設定部９３２は、該当する拘束条件の優先度に応じて運動目的を設定するようにしてもよい。

【0138】

ここまで、演算条件設定部９３２において、運動目的や拘束条件などの一般化逆動力学演算に用いる演算条件を設定する方法について例示してきたが、いずれの方法を用いるかは、手術アーム装置１００の用途などに応じてユーザーが適宜選択するようにしてもよい。もちろん、上記で例示した複数の方法を組み合わせて、演算条件設定部９３２において演算条件を設定するようにすることもできる。演算条件設定部９３２は、状態取得部９３１から受け取った手術アーム装置１００の状態と、自ら設定した運動条件および拘束条件を、仮想力推定部９３３に送信する。

【0139】

仮想力推定部９３３は、上述した仮想力算出処理に従って一般化逆動力学を用いた演算を実施して、全身協調制御に関する仮想力ｆ_vを算出する。仮想力推定部９９３は、算出した仮想力ｆ_vを実在力算出部９３４に送信する。

【0140】

実在力算出部９３４は、上述した実在力算出処理に従って、仮想力による一般化力τ_v（＝Ｊ_v ^Tｆ_v）を関節部に生じる発生トルクτ_aと外力ｆ_eとで実現するための条件を２次計画問題として解くことによって、実在力τ_aを算出する。実在力算出部９３４は、算出した実在力τ_aを、トルク指令値として理想関節制御部９４０に送信する。

【0141】

上式（５）に示した操作空間（例えば、多リンク構造体の関節空間）のバイアス加速度ｃは、重力、コリオリ力、及び遠心力に相当するｂ項を含む。手術アーム装置１００が可動式の手術台に取り付けられている場合、ローカル座標系上のＺ軸方向が手術台３００（又は、その患者支持面）の傾斜に伴って変化するため、手術アーム１２０に作用する重力がローカル座標系上のＺ方向と一致するとは限らなくなる。上述したように、本実施形態では、重力ｇとコリオリ力及び遠心力ｖを、上式（６）に示したように、（傾斜検出部９１１で検出される手術アーム装置１００の傾斜角に相当する）関節角又は関節角速度の関数として扱うようにした。これによって、仮想力推定部９３３は、傾斜検出部９１１で取得した手術アーム装置１００本体の現在の傾斜角度に応じて正しい重力方向に作用する重力項を含む操作空間バイアス加速度ｃで構成される運動方程式を用いた一般化逆動力学に基づく演算によって、正しい重力方向で重力補償が施された仮想力を推定することができる。ひいては、実在力算出部９３４は、正しい重力方向で重力補償が施された実在力を算出して、理想関節制御部９４０に渡すことができる。

【0142】

理想関節制御部９４０は、関節部９１２に対する力制御を実施する際に、関節部９１２を駆動するアクチュエータの摩擦や慣性などのモデル化や同定が困難となるダイナミクスのパラメータに起因する外乱の影響を回避して、理論モデル通りの応答を行なうようにアクチュエータの応答を修正するための機能モジュールである。理想関節制御部９４０は、外乱推定部９４１と、指令値算出部９４２を備えている。

【0143】

外乱推定部９４１は、関節状態検出部９１２－２（アクチュエータ７００の減速機７０２の出力軸に取り付けられたエンコーダ７０３）によって検出された回転角度ｑから算出される回転角速度に基づいて、関節トルクに作用するトルクを推定し、この推定トルクとトルク指令値τとの差分から、外乱トルクτ_dを推定する。

【0144】

指令値算出部９４２は、外乱推定部９４１によって推定された外乱トルクτ_dを用いて、関節部９１２に生じさせるトルク指令値τを算出する。具体的には、指令値算出部９４２は、上式（１５）で表される関節部９１２の理想応答モデルから算出されるトルク目標値τ^refに、外乱トルクτ_dを加算して、トルク指令値τを算出する。そして、指令値算出部９４２は、算出したトルク指令値τを、駆動制御部９１３に送信する。駆動制御部９１３は、トルク指令値τに従って、関節部９１２における回転角度及び発生トルクを制御する。

【0145】

以上説明してきたように、制御システム９００は、手術アーム装置１００側の関節状態と、運動目的及び拘束条件とに基づいて、手術アーム１２０の全身協調制御と、関節部９１２の理想関節制御に関する各種演算を実行して、演算結果として得られるトルク指令値τを手術アーム装置１００に送信する。一方、手術アーム装置１００は、受信したトルク指令値τに従って手術アーム１２０を構成する関節部９１２の駆動を制御するとともに、駆動中及び駆動後の関節部９１２の関節状態を検出して制御システム９００に送信する。手術アーム装置１００が駆動中は、制御システム９００と手術アーム装置１００間のこのようなやりとりが継続的に繰り返し行なわれる。

【産業上の利用可能性】

【0146】

以上、特定の実施形態を参照しながら、本明細書で開示する技術について詳細に説明してきた。しかしながら、本明細書で開示する技術の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。

【0147】

本明細書では、本明細書で開示する技術を主に手術アーム装置に適用した実施形態を中心に説明してきたが、本明細書で開示する技術の要旨はこれに限定されるものではなく、手術以外の医療用途、あるいは医療以外のさまざまな分野で利用される力制御型のアーム装置にも同様に適用することができる。

【0148】

要するに、例示という形態により本明細書で開示する技術について説明してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本明細書で開示する技術の要旨を判断するためには、特許請求の範囲を参酌すべきである。

【0149】

なお、本明細書の開示の技術は、以下のような構成をとることも可能である。
（１）手術アーム装置を制御する制御システムであって、
前記手術アーム装置の傾斜情報を取得する傾斜情報取得部と、
前記傾斜情報に応じた重力補償を含む演算条件を設定する設定部と、
前記演算条件に従って、前記手術アーム装置に対する指令値を算出する演算部と、
を具備する制御システム。
（２）前記傾斜情報取得部は、前記手術アーム装置が取り付けられた手術台の傾きに起因する傾斜情報を取得する、
上記（１）に記載の制御システム。
（３）前記傾斜情報は角度情報である、
上記（２）に記載の制御システム。
（４）前記傾斜情報取得部は、前記手術台の患者支持面の傾斜角に関する情報を取得する、
上記（２）に記載の制御システム。
（５）前記手術アーム装置は、複数のリンクが関節部により連結して構成される多リンク構造体からなり、
前記演算条件設定部は、一般化逆動力学において、前記多リンク構造体に作用する力と前記多リンク構造体の加速度との慣性を記述する操作空間上での、前記多リンク構造体の運動目的及び拘束条件を設定し、
前記演算部は、前記運動目的を表す前記操作空間加速度を実現するために前記操作空間に作用する仮想力を、前記傾斜情報に応じた重力補償を考慮した操作空間バイアス加速度の項を含む前記操作空間に関する運動方程式に基づいて算出するとともに、前記拘束条件を考慮して前記仮想力を実在力に変換し、前記実在力に基づいて前記関節部を駆動するためのトルク指令値を算出する、
上記（１）乃至（４）のいずれかに記載の制御システム。
（６）前記演算部は、前記関節部が理論応答モデルから逸脱しないように、前記実在力に対して修正をさらに加える、
上記（４）に記載の制御システム。
（７）前記演算部は、前記実在力と前記関節部に作用する外トルクに応じて前記理論応答モデルに基づいて算出されるトルク目標値を、前記関節部の外乱トルクを用いて修正して、トルク指令値を算出する、
上記（６）に記載の制御システム。
（８）前記演算部は、前記関節部の角速度から前記関節部に作用したトルクを推定し、前記推定したトルクとトルク指令値との差分に基づいて前記外乱トルクを推定する、
上記（７）に記載の制御システム。
（９）手術アーム装置を制御する制御方法であって、
前記手術アーム装置の傾斜情報を取得する傾斜情報取得ステップと、
前記傾斜情報に応じた重力補償を含む演算条件を設定する設定ステップと、
前記演算条件に従って、前記手術アーム装置に対する指令値を算出する演算ステップと、
を有する制御方法。
（１０）複数のリンクが関節部により連結して構成される多リンク構造体と、
前記多リンク構造体の傾斜情報を取得する傾斜情報取得部と、
前記関節部の状態を検出する関節状態検出部と、
前記傾斜情報と前記関節部の状態に応じて算出される指令値に従って前記関節部の駆動を制御する駆動制御部と、
を具備する手術アームシステム。
（１１）前記傾斜情報取得部は、前記手術アーム装置が取り付けられた手術台の傾きに起因する傾斜情報を取得する、
上記（１０）に記載の手術アームシステム。
（１２）前記傾斜情報は角度情報である、
上記（１１）に記載の手術アームシステム。
（１３）前記傾斜情報取得部は、前記手術台の患者支持面の傾斜角に関する情報を取得する、
上記（１１）に記載の手術アームシステム。
（１４）前記駆動制御部は、一般化逆動力学において、前記多リンク構造体に作用する力と前記多リンク構造体の加速度との慣性を記述する操作空間上で、前記多リンク構造体の運動目的を表す操作空間加速度を実現するために、前記傾斜情報に応じた重力補償を考慮した操作空間バイアス加速度の項を含む前記操作空間に関する運動方程式に基づいて算出された前記操作空間に作用する仮想力を変換した実在力に基づくトルク指令値に従って、前記関節部を駆動する、
上記（１０）乃至（１３）のいずれかに記載の手術アームシステム。
（１５）前記駆動制御部は、前記関節部が理論応答モデルから逸脱しないように修正が加えられた実在力に基づくトルク指令値に従って、前記関節部を駆動する、
上記（１４）に記載の手術アームシステム。
（１６）前記実在力と前記関節部に作用する外トルクに応じて前記理論応答モデルに基づいて算出されるトルク目標値を、前記関節部に作用した推定トルクとトルク指令値との差分に基づく外乱トルクを用いて修正したトルク指令値に従って、前記関節部を駆動する、
上記（１５）に記載の手術アームシステム。

【符号の説明】

【0150】

１００…手術アーム装置
１１０…エンドエフェクター、１１１…手首関節
１２０…手術アーム
２００…台車、３００…手術台
６００…制御システム
６０１…状態取得部、６０２…演算条件設定部
６０３…力演算部、６０４…指令値変換部
６１１…傾斜検出部、６１２…関節部
６１２－１…関節状態検出部、６１２－２、関節駆動部
６１３…駆動制御部
７００…アクチュエータ、７０１…モータ、７０２…減速機
７０３…エンコーダ、７０４…トルク・センサ
８０１…演算器（理想応答モデル）、８０２…演算器（トルク算出）
８０３…関節（伝達関数１／Ｊ_n）、８０４…演算器（微分演算）
８１０…外乱オブザーバ、８１１…演算器（作用トルク推定）
８１２…演算器（ローパス・フィルター）
９００…制御システム
９１１…傾斜検出部、９１２…関節部、
９１２－１…関節状態検出部、９１２－２…関節駆動部
９１３…駆動制御部
９２０…表示装置
９３０…全身協調制御部
９３１…状態取得部、９３２…演算条件設定部
９３３…仮想力推定部、９３４…実在力算出部
９４０…理想関節制御部
９４１…外乱推定部、９４２…指令値算出部
９５０…入力部、９６０…記憶部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【手続補正書】

【提出日】2022-02-08

【手続補正1】

【補正対象書類名】特許請求の範囲

【補正対象項目名】全文

【補正方法】変更

【補正の内容】

【特許請求の範囲】

【請求項1】

アーム装置を制御する制御システムであって、
前記アーム装置が取り付けられた台車の傾きに起因する前記アーム装置の傾斜情報を取得する傾斜情報取得部と、
前記傾斜情報に応じた重力補償を含む力制御方式の演算条件を設定する設定部と、
前記演算条件に従って、前記アーム装置に対する指令値を算出する演算部と、
を具備する制御システム。

【請求項2】

前記傾斜情報は角度情報である、
請求項１に記載の制御システム。

【請求項3】

前記傾斜情報取得部は、前記台車の支持面の傾斜角に関する情報を取得する、
請求項１に記載の制御システム。

【請求項4】

前記アーム装置は、複数のリンクが関節部により連結して構成される多リンク構造体からなり、
前記演算条件設定部は、一般化逆動力学において、前記多リンク構造体に作用する力と前記多リンク構造体の加速度との慣性を記述する操作空間上での、前記多リンク構造体の運動目的及び拘束条件を設定し、
前記演算部は、前記運動目的を表す前記操作空間加速度を実現するために前記操作空間に作用する仮想力を、前記傾斜情報に応じた重力補償を考慮した操作空間バイアス加速度の項を含む前記操作空間に関する運動方程式に基づいて算出するとともに、前記拘束条件を考慮して前記仮想力を実在力に変換し、前記実在力に基づいて前記関節部を駆動するためのトルク指令値を算出する、
請求項１に記載の制御システム。

【請求項5】

前記演算部は、前記関節部が理論応答モデルから逸脱しないように、前記実在力に対して修正をさらに加える、
請求項４に記載の制御システム。

【請求項6】

前記演算部は、前記実在力と前記関節部に作用する外トルクに応じて前記理論応答モデルに基づいて算出されるトルク目標値を、前記関節部の外乱トルクを用いて修正して、トルク指令値を算出する、
請求項４に記載の制御システム。

【請求項7】

前記演算部は、前記関節部の角速度から前記関節部に作用したトルクを推定し、前記推定したトルクとトルク指令値との差分に基づいて前記外乱トルクを推定する、
請求項６に記載の制御システム。

【請求項8】

制御システムがアーム装置を制御する制御方法であって、
前記制御システムの傾斜情報取得部が、アーム装置が取り付けられた台車の傾きに起因する前記アーム装置の傾斜情報を取得する傾斜情報取得ステップと、
前記制御システムの設定部が、前記傾斜情報に応じた重力補償を含む力制御方式の演算条件を設定する設定ステップと、
前記制御システムの演算部が、前記演算条件に従って、前記アーム装置に対する指令値を算出する演算ステップと、
を有する制御方法。

【請求項9】

複数のリンクが関節部により連結して構成される多リンク構造体からなるアームと、
前記アーム装置が取り付けられた台車の傾きに起因する前記多リンク構造体の傾斜情報を取得する傾斜情報取得部と、
前記関節部の状態を検出する関節状態検出部と、
前記傾斜情報に応じた重力補償を含む力制御方式の演算条件と前記関節部の状態に応じて算出される指令値に従って前記関節部の駆動を制御する駆動制御部と、
を具備するアームシステム。

【請求項10】

前記傾斜情報は角度情報である、
請求項９に記載のアームシステム。

【請求項11】

前記傾斜情報取得部は、前記台車の支持面の傾斜角に関する情報を取得する、
請求項９に記載のアームシステム。

【請求項12】

前記駆動制御部は、一般化逆動力学において、前記多リンク構造体に作用する力と前記多リンク構造体の加速度との慣性を記述する操作空間上で、前記多リンク構造体の運動目的を表す操作空間加速度を実現するために、前記傾斜情報に応じた重力補償を考慮した操作空間バイアス加速度の項を含む前記操作空間に関する運動方程式に基づいて算出された前記操作空間に作用する仮想力を変換した実在力に基づくトルク指令値に従って、前記関節部を駆動する、
請求項９に記載のアームシステム。

【請求項13】

前記駆動制御部は、前記関節部が理論応答モデルから逸脱しないように修正が加えられた実在力に基づくトルク指令値に従って、前記関節部を駆動する、
請求項１２に記載のアームシステム。

【請求項14】

前記実在力と前記関節部に作用する外トルクに応じて前記理論応答モデルに基づいて算出されるトルク目標値を、前記関節部に作用した推定トルクとトルク指令値との差分に基づく外乱トルクを用いて修正したトルク指令値に従って、前記関節部を駆動する、
請求項１３に記載のアームシステム。