JP6685052B2

JP6685052B2 - 推定装置、推定方法およびプログラム

Info

Publication number: JP6685052B2
Application number: JP2018161363A
Authority: JP
Inventors: 耕太郎只野
Original assignee: Riverfield Inc
Current assignee: Riverfield Inc
Priority date: 2018-08-30
Filing date: 2018-08-30
Publication date: 2020-04-22
Anticipated expiration: 2038-08-30
Also published as: CN112004495A; US11890062B2; EP3735930A1; CN112004495B; US20210030484A1; WO2020045538A1; EP3735930A4; JP2020031902A

Description

本発明は、内視鏡外科手術等に用いられる手術支援ロボットに関する推定装置、推定方法およびプログラムに関する。

近年、手術支援ロボットを用いた内視鏡外科手術が普及しつつある。内視鏡外科手術では、手術支援ロボットに取り付けられた腹腔鏡や内視鏡、鉗子など（以下、「術具」とも表記する。）が用いられる。これらの手術支援ロボットにおいては、手術支援ロボットに設定された座標系に基づいて術具の移動を制御する技術が提案されている（例えば、特許文献１参照。）。

特開２０１８−０８８９９６号公報

上述の特許文献１では、被検体の臓器モデル上に術具の先端位置を位置合わせする技術であって、術具の先端に臓器の内壁が接触した場合に、術具の先端の実座標と、対応するモデル座標との変換式を、新たな変換式に更新する技術が提案されている。

しかしながら、特許文献１に記載の技術では、手術支援ロボットにおける不動点（以下、「ピボット位置」とも表記する。）と、術具を被検体の体内に挿入する孔の位置と、の位置合わせ精度を保ちにくいという問題があった。言い換えると、手術支援ロボットに把持された術具におけるピボット位置と、被検体に配置されるとともに術具が挿通されるトロッカーの孔の位置と、の位置合わせ精度を保ちにくいという問題があった。

つまり、特許文献１に記載の技術では、術具の先端という固定された位置の推定を行うことができるが、術具におけるピボット位置については推定を行うことができない。また、術具における先端位置に対するピボット位置（相対位置）は、手術支援ロボットや被検体や術具の相対的な位置や相対的な姿勢によって変化するため、先端位置に基づいてピボット位置を推定することも困難である。そのため、内視鏡におけるピボット位置と、被検体に配置されたトロッカーの孔の位置と、の位置合わせ精度を保つことは困難であった。

本発明は、上記の課題を解決するためになされたものであって、手術支援ロボットに把持された術具におけるピボット位置の推定を可能とする推定装置、推定方法およびプログラムを提供することを目的とする。

上記目的を達成するために、本発明は、以下の手段を提供する。
本発明の第１の態様の推定装置は、関節を有するアームに把持された術具におけるピボット位置の推定を行う推定装置であって、前記術具における前記アームに把持される後端側の基準点から仮のピボット位置までの長さ、または、前記後端とは反対側の端部である先端側の基準点から前記仮のピボット位置までの長さであって、予め定められた値を有する仮距離が記憶される記憶部と、前記アームにおける前記術具を把持する把持部または前記術具の向きに関する情報であるベクトル情報、前記術具が移動した際の前記術具における前記仮のピボット位置の移動に関する第１移動ベクトル情報、および、前記術具における前記仮のピボット位置とは異なる位置の移動に関する第２移動ベクトル情報を取得する取得部と、前記ベクトル情報に基づいて前記術具の軸線方向を算出し、前記第１移動ベクトルの情報、前記第２移動ベクトルの情報、および前記術具の軸線方向に基づいて、前記術具の軸線方向に対する垂直成分である第１垂直ベクトルおよび第２垂直ベクトルをそれぞれ算出する垂直ベクトル算出部と、算出された前記第１垂直ベクトルおよび前記第２垂直ベクトルの内積の値を算出する内積算出部と、算出された前記内積の値に所定の係数を乗じた値を、前記記憶部に記憶されている前記仮距離に加えて前記仮距離を更新する修正演算部と、が設けられていることを特徴とする。

本発明の第２の態様の推定方法は、関節を有するアームに把持された術具におけるピボット位置の推定を行う推定方法であって、前記アームにおける前記術具を把持する把持部または前記術具の向きに関する情報であるベクトル情報を取得するステップと、前記術具が移動した際の前記術具における仮のピボット位置の移動に関する第１移動ベクトルの情報、および、前記術具における前記把持部に把持される後端部の移動に関する第２移動ベクトルの情報を取得するステップと、取得した前記ベクトル情報に基づいて前記術具の軸線方向を算出し、前記第１移動ベクトルの情報、前記第２移動ベクトルの情報、および前記術具の軸線方向に基づいて、前記術具の軸線方向に対する垂直成分である第１垂直ベクトルおよび第２垂直ベクトルをそれぞれ算出するステップと、算出された前記第１垂直ベクトルおよび前記第２垂直ベクトルの内積の値を算出するステップと、算出された前記内積の値に所定の係数を乗じた値を、前記術具における前記把持部に把持される後端側の基準点から前記仮のピボット位置までの長さ、または、前記後端とは反対側の端部である先端側の基準点から前記仮のピボット位置までの長さであって、予め定められた値を有する仮距離に加えて前記仮距離を更新するステップと、を有することを特徴とする。

本発明の第３の態様のプログラムは、関節を有するアームに把持された術具におけるピボット位置の推定を行うプログラムであって、コンピュータに、前記アームにおける前記術具を把持する把持部または前記術具の向きに関する情報であるベクトル情報を取得する機能と、前記術具が移動した際の前記術具における仮のピボット位置の移動に関する第１移動ベクトルの情報、および、前記術具における前記把持部に把持される後端部の移動に関する第２移動ベクトルの情報を取得する機能と、取得した前記ベクトル情報に基づいて前記術具の軸線方向を算出し、前記第１移動ベクトルの情報、前記第２移動ベクトルの情報、および前記術具の軸線方向に基づいて、前記術具の軸線方向に対する垂直成分である第１垂直ベクトルおよび第２垂直ベクトルをそれぞれ算出する機能と、算出された前記第１垂直ベクトルおよび前記第２垂直ベクトルの内積の値を算出する機能と、算出された前記内積の値に所定の係数を乗じた値を、前記術具における前記把持部に把持される後端側の基準点から前記仮のピボット位置までの長さ、または、前記後端とは反対側の端部である先端側の基準点から前記仮のピボット位置までの長さであって、予め定められた値を有する仮距離に加えて前記仮距離を更新する機能と、を実現させることを特徴とする。

本発明の第１の態様に係る推定装置、第２の態様に係る推定方法、および、第３の態様に係るプログラムによれば、術具における仮のピボット位置の移動ベクトルの垂直成分（第１垂直ベクトル）と、仮のピボット位置以外の位置の移動ベクトルの垂直成分（第２垂直ベクトル）と、の内積に基づいて仮距離を更新することにより、術具におけるピボット位置の推定が可能となる。

上記発明において前記術具における前記仮のピボット位置とは異なる位置は、前記術具における前記後端部であることが好ましい。
このように術具における後端部を上述の異なる位置とすることにより、後端部以外の位置を上述の異なる位置とした場合と比較して、術具におけるピボット位置の推定が容易となる。

上記発明において前記ベクトル情報は、角度センサにより測定された前記関節における回転角度であり、前記垂直ベクトル演算部は、前記角度センサから取得された前記回転角度に基づいて、前記術具の軸線方向を算出することが好ましい。

このようにベクトル情報として角度センサにより測定されたアームの関節角度を用いることにより、把持部または術具の向きを直接測定する場合と比較して、ベクトル情報の取得が容易になる。

上記発明において前記第１移動ベクトル情報および前記第２移動ベクトル情報は、所定の時間間隔をあけて測定された前記関節における回転角度を含み、前記垂直ベクトル演算部は、前記所定の時間間隔をあけて測定された前記関節における回転角度に少なくとも基づいて、前記第１垂直ベクトルおよび前記第２垂直ベクトルを算出することが好ましい。

このように第１移動ベクトル情報および第２移動ベクトル情報として、少なくとも所定の時間間隔をあけて測定された関節における回転角度を用いることにより、第１移動ベクトル情報および第２移動ベクトル情報を直接測定する場合と比較して、第１移動ベクトル情報および第２移動ベクトル情報の取得が容易になる。

上記発明において前記取得部は、予め定められた所定のサンプリング周期ごとに前記ベクトル情報、前記第１移動ベクトル情報、および、前記第２ベクトル情報を取得し、前記垂直ベクトル算出部は、前記所定のサンプリング周期ごとに前記第１垂直ベクトルおよび前記第２垂直ベクトルを算出し、前記内積算出部は、前記所定のサンプリング周期ごとに前記内積の値を算出し、前記修正演算部は、前記所定のサンプリング周期ごとに前記仮距離を更新することが好ましい。

このように所定のサンプリング周期ごとに仮距離の更新を繰り返し行うことにより、繰り返しを行わない場合と比較して、術具におけるピボット位置の推定精度が高められやすい。さらに、何らかの原因で術具におけるピボット位置が移動して推定精度が悪化した場合であっても、サンプリング周期ごとに仮距離の更新が繰り返されることにより、術具におけるピボット位置の推定精度を再び高めることが可能となる。

本発明の第１の態様に係る推定装置、第２の態様に係る推定方法、および、第３の態様に係るプログラムによれば、第１垂直ベクトルおよび第２垂直ベクトルの内積に基づいて仮距離を更新してピボット位置の推定を行うことにより、手術支援ロボットに把持された術具におけるピボット位置の推定が可能になるという効果を奏する。

本発明の一実施形態に係る推定装置および推定装置が適用されるアーム装置の概略を説明する模式図である。図１の推定装置の構成を説明するブロック図である。ピボット位置を記述する式を説明する図である。ピボット位置の推定を行う演算処理について説明するフローチャートである。ピボット位置の推定を行う演算処理を説明する模式図である。ピボット位置の推定を行う演算処理を説明する他の模式図である。ピボット位置の推定を行う演算処理を説明する更に別の模式図である。

以下、本発明の一実施形態に係る推定装置１０ついて図１から図７を参照しながら説明する。本実施形態では推定装置１０を、図１に示すような内視鏡外科手術に用いられる手術支援ロボットなどのアーム装置（アーム）５０に用いる例に適用して説明する。

アーム装置５０は、術具７０の位置および姿勢を変更可能に支持するものである。また、アーム装置５０は、術具７０の位置や姿勢を変更しても、術具７０がアーム装置５０に対する所定の相対位置であるピボット位置Ｐ_ｐを通過するように制御されている。ピボット位置Ｐ_ｐは、内視鏡外科手術の対象である患者の腹壁８１に配置されたトロッカー７５の配置位置と概ね一致している。

また、術具７０としては、内視鏡や鉗子など、内視鏡外科手術に用いられる各種の器具を例示することができる。本実施形態では、術具７０が内視鏡である例について説明する。

内視鏡である術具７０には、本体部７１と、筒状部７２と、が主に設けられている。本体部７１は、アーム装置５０によって把持される部分であって、筒状部７２により導かれた画像を電子信号に変換する撮像機器が収納されるものである。

筒状部７２は、筒状または棒状に延びる部材であって、トロッカー７５に挿通されて患者の腹壁８１の内部に挿入されるものである。また、筒状部７２は、先端から本体部７１へ画像の伝達が可能に形成されている。

アーム装置５０には、図１に示すように、回転部（関節）５２と、第１リンク部５５と、第２リンク部５６と、ジンバル部６１と、把持部６５と、が主に設けられている。回転部５２、第１リンク部５５および第２リンク部５６は、アーム装置５０の制御部など（図示せず）から入力される制御信号に基づいて、駆動制御されるものである。

回転部５２は、アーム装置５０が土台５１に固定される部分に配置される関節である。回転部５２としては、上下方向に延びる回転軸線まわりに回転駆動が可能な構成であればよく、具体的な構成を限定するものではない。回転部５２には、回転部５２の回転角度を検出する回転センサ（角度センサ）５２ｓが設けられている。

第１リンク部５５は、第１関節部６２と第２リンク部５６との間に配置され、アクチュエータ（図示せず）により駆動されるものである。第１リンク部５５は、一対の並行なバー５５ｂを２組有した矩形状に形成され、バー５５ｂとバー５５ｂとの交点は、一自由度の回転を許容するピン（関節）５５ｐにより結合された構成を有している。

また、第１リンク部５５には、所定のピン５５ｐにおける回転角度を検出する第１リンクセンサ（角度センサ）５５ｓが設けられている。本実施形態では、第１リンク部５５が上下方向へ延びた姿勢に配置される例に適用して説明する。

第２リンク部５６は、第１リンク部５５とジンバル部６１との間に配置され、アクチュエータ（図示せず）により駆動されるものである。第２リンク部５６は、第１リンク部５５と同様に、一対の並行なバー５６ｂを２組有した矩形状に形成され、隣接する２つのバー５６ｂは一自由度の回転を許容するピン（関節）５６ｐにより結合された構成を有している。

また、第２リンク部５６には、所定のピン５６ｐにおける回転角度を検出する第２リンクセンサ（角度センサ）５６ｓが設けられている。本実施形態では、第２リンク部５６が横方向（上下方向と交差する面に沿った方向）へ延びた姿勢に配置される例に適用して説明する。

ジンバル部６１は、第２リンク部５６と把持部６５との間に配置されるものである。ジンバル部６１は、回転軸線が互いに交差する第１関節部（関節）６２、第２関節部（関節）６３および第３関節部（関節）６４と、第１関節部６２の回転角度を検出する第１関節センサ（角度センサ）６２ｓ、第２関節部６３の回転角度を検出する第２関節センサ（角度センサ）６３ｓ、および第３関節部６４の回転角度を検出する第３関節センサ（角度センサ）６４ｓと、から主に構成されている。

第１関節部６２は、第２リンク部５６と隣接する位置に配置されるものである。第１関節部６２は、その回転軸線が水平方向から斜め上方へ延びる姿勢に配置されている。より好ましくは、水平方向から４５°の角度で斜め上方へ延びる姿勢に配置されている。

第２関節部６３は、第１関節部６２と第３関節部６４との間に配置されるものである。第３関節部６４は、把持部６５に隣接する位置に配置されるものである。なお、第１関節部６２、第２関節部６３および第３関節部６４としては、回転軸回りの回転を可能とするものであればよく、特に限定するものではない。

把持部６５は、ジンバル部６１と隣接する位置、言い換えるとアーム装置５０の先端に配置されるものである。把持部６５としては、術具７０を把持することができる構成であればよく、具体的な構成を限定するものではない。

推定装置１０は、アーム装置５０に把持された術具７０におけるピボット位置Ｐ_ｐの推定を行うものである。推定装置１０により推定された術具７０におけるピボット位置Ｐ_ｐは、例えば、アーム装置５０を制御する制御部などへ出力される。ここで、推定装置１０により推定されるピボット位置Ｐ_ｐは、アーム装置５０に関する座標系で表現されるものである。

推定装置１０は、図１および図２に示すように、ＣＰＵ（中央演算処理ユニット）、ＲＯＭ、ＲＡＭ、入出力インタフェース等を有するコンピュータ等の情報処理装置である。上述のＲＯＭ等の記憶装置に記憶されているプログラムは、ＣＰＵ、ＲＯＭ、ＲＡＭ、入出力インタフェースを協働させて、少なくとも取得部１１、記憶部１２、垂直ベクトル算出部１３、内積算出部１４、および、修正演算部１５として機能させるものである。

取得部１１は、回転センサ５２ｓ、第１リンクセンサ５５ｓ、第２リンクセンサ５６ｓ、第１関節センサ６２ｓ、第２関節センサ６３ｓおよび第３関節センサ６４ｓから出力される回転角度の情報を取得するものである。

取得部１１は、取得した回転角度の情報に基づいて、アーム装置５０の把持部６５、または、術具７０の向きを表すベクトルｎに関する情報であるベクトル情報を算出する。また、後述する仮のピボット位置Ｐ_ｐ´の移動を表す第１移動ベクトルΔＰ_ｐ´の情報、および、術具７０における把持部６５側の端部である後端部Ｐ_ｒの移動を表す第２移動ベクトルΔＰ_ｒの情報も同様に算出する。

なお、本実施形態では、第２移動ベクトルΔＰ_ｒが、術具７０における後端部Ｐ_ｒの移動を表すものに適用して説明するが、第２移動ベクトルΔＰ_ｒは後端部Ｐ_ｒにおける移動を表すものに限定されるものではなく、仮のピボット位置Ｐ_ｐ´とは異なる位置における移動を表すものであればよい。

記憶部１２は、予め定められた仮距離ｄ´の値が記憶されるものである。仮距離ｄ´は、アーム装置５０の先端位置と、術具７０の向きを表すベクトルｎとともに、アーム装置５０に対するピボット位置Ｐ_ｐの推定に用いられるものである。

具体的には仮距離ｄ´は、アーム装置５０の先端位置から仮のピボット位置Ｐ_ｐ´までの距離を表すものである。本実施形態では、アーム装置５０の先端位置が、アーム装置５０に把持される術具７０の後端側の基準点、例えば本体部７１の後端部Ｐ_ｒである例に適用して説明する。つまり、術具７０の後端部Ｐ_ｒから仮のピボット位置Ｐ_ｐ´までの長さが仮距離ｄ´となる。

ここで、仮のピボット位置Ｐ_ｐ´は、予め定められるものであり、真のピボット位置Ｐ_ｐの推定を行う際に用いられるものである。そのため、真のピボット位置Ｐ_ｐと、仮のピボット位置Ｐ_ｐ´とは、一致しているか否かを問うものではない。なお、推定を行うために仮のピボット位置Ｐ_ｐ´は、術具７０上に設定されていることが好ましい。

なお、アーム装置５０の先端位置を、術具７０の先端側の基準点、例えば先端部としてもよい。この場合、仮距離ｄ´は、術具７０の先端部から仮のピボット位置Ｐ_ｐ´までの長さが仮距離ｄ´となる。

垂直ベクトル算出部１３は、術具７０の軸線Ｌ方向を算出するものであり、第１垂直ベクトルΔＰ_ｐ⊥´および第２垂直ベクトルΔＰ_ｒ⊥を算出するものである。ここで、術具７０の軸線Ｌ方向は、ベクトル情報に基づいて算出される。また、第１垂直ベクトルΔＰ_ｐ⊥´および第２垂直ベクトルΔＰ_ｒ⊥は、それぞれ、第１移動ベクトルΔＰ_ｐ´および第２移動ベクトルΔＰ_ｒにおける軸線Ｌ方向に対する垂直成分である。

内積算出部１４は、第１垂直ベクトルΔＰ_ｐ⊥´および第２垂直ベクトルΔＰ_ｒ⊥の内積の値を算出するものである。
修正演算部１５は、内積の値に所定の係数ｋを乗じた値を、記憶部１２に記憶されている仮距離ｄ´に加える演算を行うものである。さらに、記憶されていた仮距離ｄ´の値を、当該演算により求められた新たな仮距離ｄ´の値に更新する処理を行うものである。

次に、上記の構成からなる推定装置１０におけるピボット位置Ｐ_ｐの推定を行う演算処理について説明する。まず、図３を参照しながら、ピボット位置Ｐ_ｐを記述する式について説明する。ピボット位置Ｐ_ｐは、後端部Ｐ_ｒ（アーム装置５０の先端位置）と、ベクトルｎと、距離ｄを用いて以下の式（１）により記述される。

次に、ピボット位置Ｐ_ｐの推定を行う演算処理について説明する。推定装置１０は、図４に示すピボット位置Ｐ_ｐの推定を行う演算処理が開始されると、予め定められたサンプリング周期に従って、繰り返し推定の演算処理を行う。推定の演算処理は、少なくとも、アーム装置５０の操作が終了するまで継続される。

まず、推定装置１０は、取得部１１を用いてアーム装置５０における術具７０を把持する把持部６５または術具７０の向きを表すベクトルｎに関する情報であるベクトル情報を取得する処理を行う（Ｓ１１）。具体的には、取得部１１は、回転センサ５２ｓ、第１リンクセンサ５５ｓ、第２リンクセンサ５６ｓ、第１関節センサ６２ｓ、第２関節センサ６３ｓおよび第３関節センサ６４ｓ（以後「回転センサ５２ｓ等」とも表記する。）から出力される回転角度の情報を取得する処理を行う。その後、取得した回転角度の情報に基づいて、アーム装置５０の把持部６５、または、術具７０の向きを表すベクトルｎに関する情報であるベクトル情報を算出する演算処理を行う。

次に、取得部１１は、第１移動ベクトルΔＰ_ｐ´の情報、および、第２移動ベクトルΔＰ_ｒの情報を取得する処理を行う（Ｓ１２）。具体的には、取得部は、直近に取得した回転センサ５２ｓ等から出力された回転角度の情報と、それ以前（例えば更に前のサンプリング周期）で取得した回転センサ５２ｓ等から出力された回転角度の情報と、に基づいて、第１移動ベクトルΔＰ_ｐ´の情報、および、第２移動ベクトルΔＰ_ｒの情報を算出する演算処理を行う。

その後、推定装置１０は、垂直ベクトル算出部１３により第１垂直ベクトルΔＰ_ｐ⊥´および第２垂直ベクトルΔＰ_ｒ⊥を算出する演算処理を行う（Ｓ１３）。具体的には、推定装置１０は、まずベクトルｎに関する情報であるベクトル情報に基づいて術具７０の軸線Ｌ方向を算出する演算処理を行う。次いで、第１移動ベクトルΔＰ_ｐ´の情報、第２移動ベクトルΔＰ_ｒの情報、および術具７０の軸線Ｌ方向に基づいて、術具７０の軸線Ｌ方向に対する垂直成分である第１垂直ベクトルΔＰ_ｐ⊥´および第２垂直ベクトルΔＰ_ｒ⊥をそれぞれ算出する演算処理を行う（図４参照。）。

第１垂直ベクトルΔＰ_ｐ⊥´および第２垂直ベクトルΔＰ_ｒ⊥を算出すると、推定装置１０は、内積算出部１４により第１垂直ベクトルΔＰ_ｐ⊥´および第２垂直ベクトルΔＰ_ｒ⊥の内積の値Δｄ´を算出する演算処理を行う（Ｓ１４）。

例えば、図５に示すように、仮距離ｄ´が、真の値である距離ｄよりも大きい場合、言い換えると、仮のピボット位置Ｐ_ｐ´が、ピボット位置Ｐ_ｐよりも術具７０の先端側に位置する場合には、第１垂直ベクトルΔＰ_ｐ⊥´の向きと、第２垂直ベクトルΔＰ_ｒ⊥の向きとは逆方向となる。そのため、両者の内積の値Δｄ´は負の値となる。

その一方で、図６に示すように、仮距離ｄ´が、真の値である距離ｄよりも小さい場合、言い換えると、仮のピボット位置Ｐ_ｐ´が、ピボット位置Ｐ_ｐよりも後端部Ｐ_ｒ側に位置する場合には、第１垂直ベクトルΔＰ_ｐ⊥´の向きと、第２垂直ベクトルΔＰ_ｒ⊥の向きとは同方向となる。そのため、両者の内積の値Δｄ´は正の値となる。

内積の値Δｄ´を算出すると、推定装置１０は、修正演算部１５により仮距離ｄ´を修正して更新する演算処理を行う（Ｓ１５）。具体的には、修正演算部１５は、内積の値Δｄ´に所定の係数ｋ（但しｋは正の値）を乗じた値を、記憶部１２に記憶されている仮距離ｄ´に加えて修正する演算を行う。さらに修正演算部１５は、記憶されていた仮距離ｄ´の値を、当該演算により求められた新たな仮距離ｄ´の値に更新する処理を行う。

例えば、図５に示す場合には、仮距離ｄ´を小さくする修正が行われ、真の値である距離ｄに近づく修正が行われる。その一方で、図６に示す場合には、仮距離ｄ´を大きくする修正が行われ、真の値である距離ｄに近づく修正が行われる。

所定の係数ｋは、所望の値に設定することができる。例えば、所定の係数ｋの値を大きくすると仮距離ｄ´が真の値である距離ｄに早く近づくようになるが、真の値である距離ｄに収束しにくくなる可能性がある。また、値を小さくすると、仮距離ｄ´が真の値である距離ｄにゆっくりと近づくようになるが、真の値である距離ｄに収束しやすくなる。

なお、仮距離ｄ´が術具７０の先端部から仮のピボット位置Ｐ_ｐ´までの距離を表す場合には、図７に示すように、Ｓ１５における仮距離ｄ´を修正する際の増減が逆になる。そのため、所定の係数ｋは負の値を有していることが好ましい。

上記の構成の推定装置１０によれば、術具７０における仮のピボット位置Ｐ_ｐ´の移動ベクトルの垂直成分である第１垂直ベクトルΔＰ_ｐ⊥´と、仮のピボット位置Ｐ_ｐ´以外の位置の移動ベクトルの垂直成分である第２垂直ベクトルΔＰ_ｒ⊥と、の内積に基づいて仮距離ｄ´を更新することにより、術具７０におけるピボット位置Ｐ_ｐの推定が可能となる。

術具７０における後端部Ｐ_ｒを上述の異なる位置とすることにより、後端部Ｐ_ｒ以外の位置を上述の異なる位置とした場合と比較して、術具７０におけるピボット位置Ｐ_ｐの推定が容易となる。

ベクトル情報として回転センサ５２ｓ等により測定されたアーム装置５０の関節角度を用いることにより、把持部６５または術具７０の向きを表すベクトルｎを直接測定する場合と比較して、ベクトル情報の取得が容易になる。

第１移動ベクトルΔＰ_ｐ´の情報および第２移動ベクトルΔＰ_ｒの情報として、少なくとも所定の時間間隔をあけて回転センサ５２ｓ等により測定された回転角度を用いることにより、第１移動ベクトルΔＰ_ｐ´の情報および第２移動ベクトルΔＰ_ｒの情報を直接測定する場合と比較して、第１移動ベクトルΔＰ_ｐ´の情報および第２移動ベクトルΔＰ_ｒの情報の取得が容易になる。

所定のサンプリング周期ごとに仮距離ｄ´の更新を繰り返し行うことにより、繰り返しを行わない場合と比較して、術具７０におけるピボット位置Ｐ_ｐの推定精度が高められやすい。さらに、何らかの原因で術具７０におけるピボット位置Ｐ_ｐが移動して推定精度が悪化した場合であっても、サンプリング周期ごとに仮距離ｄ´の更新が繰り返されることにより、術具７０におけるピボット位置Ｐ_ｐの推定精度を再び高めることが可能となる。

なお、本発明の技術範囲は上記実施形態に限定されるものではなく、本発明の趣旨を逸脱しない範囲において種々の変更を加えることが可能である。例えば、上記の実施の形態において推定装置１０は、アーム装置５０およびその制御部とは別に配置される例に適用して説明したが、推定装置１０は、アーム装置５０およびその制御部の内部に組み込まれたものであってもよい。

１０…推定装置、１１…取得部、１２…記憶部、１３…垂直ベクトル算出部、１４…内積算出部、１５…修正演算部、５０…アーム装置（アーム）、５２…回転部（関節）、５２ｓ…回転センサ（角度センサ）、５５ｐ，５６ｐ…ピン（関節）、５５ｓ…第１リンクセンサ（角度センサ）、５６ｓ…第２リンクセンサ（角度センサ）、６２…第１関節部（関節）、６３…第２関節部（関節）、６４…第３関節部（関節）、６２ｓ…第１関節センサ（角度センサ）、６３ｓ…第２関節センサ（角度センサ）、６４ｓ…第３関節センサ（角度センサ）、７０…術具、Ｐ_ｐ…ピボット位置、Ｐ_ｐ´…仮のピボット位置、ΔＰ_ｐ´…第１移動ベクトル、ΔＰ_ｒ…第２移動ベクトル、ΔＰ_ｐ⊥´…第１垂直ベクトル、ΔＰ_ｒ⊥…第２垂直ベクトル、ｄ´…仮距離

Claims

関節を有するアームに把持された術具におけるピボット位置の推定を行う推定装置であって、
前記術具における前記アームに把持される後端側の基準点から仮のピボット位置までの長さ、または、前記後端とは反対側の端部である先端側の基準点から前記仮のピボット位置までの長さであって、予め定められた値を有する仮距離が記憶される記憶部と、
前記アームにおける前記術具を把持する把持部または前記術具の向きに関する情報であるベクトル情報、前記術具が移動した際の前記術具における前記仮のピボット位置の移動に関する第１移動ベクトル情報、および、前記術具における前記仮のピボット位置とは異なる位置の移動に関する第２移動ベクトル情報を取得する取得部と、
前記ベクトル情報に基づいて前記術具の軸線方向を算出し、前記第１移動ベクトルの情報、前記第２移動ベクトルの情報、および前記術具の軸線方向に基づいて、前記仮のピボット位置における前記術具の軸線方向に対する垂直成分である第１垂直ベクトルおよび前記異なる位置における前記軸線方向に対する垂直成分である第２垂直ベクトルをそれぞれ算出する垂直ベクトル算出部と、
算出された前記第１垂直ベクトルおよび前記第２垂直ベクトルの内積の値を算出する内積算出部と、
算出された前記内積の値に所定の係数を乗じた値を、前記記憶部に記憶されている前記仮距離に加えて前記仮距離を更新する修正演算部と、
が設けられていることを特徴とする推定装置。
前記術具における前記仮のピボット位置とは異なる位置は、前記術具における後端部であることを特徴とする請求項１記載の推定装置。
前記ベクトル情報は、角度センサにより測定された前記関節における回転角度であり、
前記垂直ベクトル算出部は、前記角度センサから取得された前記回転角度に基づいて、前記術具の軸線方向を算出することを特徴とする請求項１または２に記載の推定装置。
前記第１移動ベクトル情報および前記第２移動ベクトル情報は、所定の時間間隔をあけて測定された前記関節における回転角度を含み、
前記垂直ベクトル算出部は、前記所定の時間間隔をあけて測定された前記関節における回転角度に少なくとも基づいて、前記第１垂直ベクトルおよび前記第２垂直ベクトルを算出することを特徴とする請求項１から３のいずれか１項に記載の推定装置。
前記取得部は、予め定められた所定のサンプリング周期ごとに前記ベクトル情報、前記第１移動ベクトル情報、および、前記第２移動ベクトル情報を取得し、
前記垂直ベクトル算出部は、前記所定のサンプリング周期ごとに前記第１垂直ベクトルおよび前記第２垂直ベクトルを算出し、
前記内積算出部は、前記所定のサンプリング周期ごとに前記内積の値を算出し、
前記修正演算部は、前記所定のサンプリング周期ごとに前記仮距離を更新することを特徴とする請求項１から４のいずれか１項に記載の推定装置。
関節を有するアームに把持された術具におけるピボット位置の推定を行う推定方法であって、
前記アームにおける前記術具を把持する把持部または前記術具の向きに関する情報であるベクトル情報を取得するステップと、
前記術具が移動した際の前記術具における仮のピボット位置の移動に関する第１移動ベクトルの情報、および、前記術具における前記把持部に把持される後端部の移動に関する第２移動ベクトルの情報を取得するステップと、
取得した前記ベクトル情報に基づいて前記術具の軸線方向を算出し、前記第１移動ベクトルの情報、前記第２移動ベクトルの情報、および前記術具の軸線方向に基づいて、前記仮のピボット位置における前記術具の軸線方向に対する垂直成分である第１垂直ベクトルおよび前記異なる位置における前記軸線方向に対する垂直成分である第２垂直ベクトルをそれぞれ算出するステップと、
算出された前記第１垂直ベクトルおよび前記第２垂直ベクトルの内積の値を算出するステップと、
算出された前記内積の値に所定の係数を乗じた値を、前記術具における前記把持部に把持される後端側の基準点から前記仮のピボット位置までの長さ、または、前記後端とは反対側の端部である先端側の基準点から前記仮のピボット位置までの長さであって、予め定められた値を有する仮距離に加えて前記仮距離を更新するステップと、
を有することを特徴とする推定方法。
関節を有するアームに把持された術具におけるピボット位置の推定を行うプログラムであって、
コンピュータに
前記アームにおける前記術具を把持する把持部または前記術具の向きに関する情報であるベクトル情報を取得する機能と、
前記術具が移動した際の前記術具における仮のピボット位置の移動に関する第１移動ベクトルの情報、および、前記術具における前記把持部に把持される後端部の移動に関する第２移動ベクトルの情報を取得する機能と、
取得した前記ベクトル情報に基づいて前記術具の軸線方向を算出し、前記第１移動ベクトルの情報、前記第２移動ベクトルの情報、および前記術具の軸線方向に基づいて、前記仮のピボット位置における前記術具の軸線方向に対する垂直成分である第１垂直ベクトルおよび前記異なる位置における前記軸線方向に対する垂直成分である第２垂直ベクトルをそれぞれ算出する機能と、
算出された前記第１垂直ベクトルおよび前記第２垂直ベクトルの内積の値を算出する機能と、
算出された前記内積の値に所定の係数を乗じた値を、前記術具における前記把持部に把持される後端側の基準点から前記仮のピボット位置までの長さ、または、前記後端とは反対側の端部である先端側の基準点から前記仮のピボット位置までの長さであって、予め定められた値を有する仮距離に加えて前記仮距離を更新する機能と、
を実現させることを特徴とするプログラム。