JP2022002693A - 連続体ロボット及びその制御方法、並びに、プログラム - Google Patents
連続体ロボット及びその制御方法、並びに、プログラム Download PDFInfo
- Publication number
- JP2022002693A JP2022002693A JP2021099452A JP2021099452A JP2022002693A JP 2022002693 A JP2022002693 A JP 2022002693A JP 2021099452 A JP2021099452 A JP 2021099452A JP 2021099452 A JP2021099452 A JP 2021099452A JP 2022002693 A JP2022002693 A JP 2022002693A
- Authority
- JP
- Japan
- Prior art keywords
- bending angle
- target
- displacement
- section
- curved section
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1615—Programme controls characterised by special kind of manipulator, e.g. planar, scara, gantry, cantilever, space, closed chain, passive/active joints and tendon driven manipulators
- B25J9/1625—Truss-manipulator for snake-like motion
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
- A61B1/005—Flexible endoscopes
- A61B1/008—Articulations
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
- A61B1/005—Flexible endoscopes
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J18/00—Arms
- B25J18/06—Arms flexible
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/06—Programme-controlled manipulators characterised by multi-articulated arms
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/06—Programme-controlled manipulators characterised by multi-articulated arms
- B25J9/065—Snake robots
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
- B25J9/104—Programme-controlled manipulators characterised by positioning means for manipulator elements with cables, chains or ribbons
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1635—Programme controls characterised by the control loop flexible-arm control
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/40—Robotics, robotics mapping to robotics vision
- G05B2219/40234—Snake arm, flexi-digit robotic manipulator, a hand at each end
Abstract
【課題】連続体ロボットの操作性の向上を図ることができる仕組みを提供する。【解決手段】入力された遠位湾曲区間の第1の目標湾曲角度に応じて設定される、連続体ロボットの前進移動に対応した追従湾曲区間の湾曲角度に関する第1の湾曲角度のプロファイルに従って、追従湾曲区間の湾曲角度が第1の目標湾曲角度に至る。遠位湾曲区間を第1の目標湾曲角度に湾曲した後に、第1の湾曲角度のプロファイルに従って追従湾曲区間が湾曲および前進移動し、前進移動の移動量が追従湾曲区間の長さに相当する第1の移動量に至る前に、遠位湾曲区間の目標湾曲角度が、第1の目標湾曲角度から第2の目標湾曲角度へ変更された場合、第1の湾曲角度のプロファイルとは異なる第2の湾曲角度のプロファイルが設定され、連続体ロボットのさらなる前進駆動により第2の湾曲角度のプロファイルに従って追従湾曲区間の湾曲角度が第2の目標湾曲角度に至る。【選択図】図7
Description
本発明は、ワイヤ(線状部材)が駆動することによって湾曲する湾曲区間が複数設けられた湾曲可能部を備える連続体ロボット及びその制御方法、並びに、当該連続体ロボットとしてコンピュータを機能させるためのプログラムに関するものである。
連続体ロボットは、コンティニュウムロボットとも呼ばれ、柔軟性のある構造を持つ複数の湾曲区間が設けられた湾曲可能部を備えて構成されており、その湾曲区間を変形させることによって形状が制御される。この連続体ロボットは、剛体リンクによって構成される剛体リンクロボットに対して、主に2つの優位性を持つ。1つ目は、剛体リンクロボットがはまり込んでしまうような狭い空間や散乱物のある環境の中で、連続体ロボットは、曲線に沿って移動可能である。2つ目は、連続体ロボットは、本質的な柔らかさを有するため、脆弱な対象物に損傷を与えることなく操作することができる。そこでは、剛体リンクロボットで必要とされる外力の検出などは必ずしも必要とされない。この特徴を生かし、連続体ロボットは、内視鏡のシースやカテーテルなどの医療の分野や、レスキューロボットなどの極限作業ロボットへの応用が期待されている。
特許文献1には、内視鏡として用いられる連続体ロボットが空間に侵入するための制御方法が記載されている。具体的に、特許文献1では、隣接する湾曲区間の全ての組において、連続体ロボットの基台部の前進に伴って前方湾曲区間の湾曲形状を後続湾曲区間の湾曲形状にするように制御することで、湾曲形状を連続的に伝播させている。
特許文献1では、連続体ロボットの基台部が湾曲区間の長さを前進するごとに、前方の湾曲区間の湾曲形状を後続の湾曲区間に伝播させる制御方法を開示しているが、湾曲区間の長さに満たない基台部の変位の際に前方の湾曲区間の湾曲形状が変更された場合の後続の湾曲区間への湾曲形状の伝播については、何ら想定されていない。このため、特許文献1に記載の技術では、操縦者の意図と連続体ロボットの形状との乖離が大きくなったり、目標経路への連続体ロボットの侵入が困難となったりして、連続体ロボットの操作性の向上が図れないという問題があった。
本発明は、このような問題点に鑑みてなされたものであり、連続体ロボットの操作性の向上を図ることができる仕組みを提供することを目的とする。
本発明の連続体ロボットは、基台部と、遠位用の線状部材が駆動することによって湾曲する遠位湾曲区間と、前記遠位湾曲区間と前記基台部との間に設けられ、追従用の線状部材が駆動することによって湾曲する追従湾曲区間と、前記遠位用の線状部材および前記追従用の線状部材を独立に駆動する駆動手段と、前記基台部、前記遠位湾曲区間、ならびに前記追従湾曲区間を一体にして前進移動させる移動手段と、制御部と、を備えた連続体ロボットであって、前記制御部は、入力された前記遠位湾曲区間の第1の目標湾曲角度に応じて設定される、前記連続体ロボットの前進移動に対応した前記追従湾曲区間の湾曲角度に関する第1の湾曲角度のプロファイルに従って、前記追従湾曲区間の湾曲角度が前記第1の目標湾曲角度に至るよう構成されており、前記遠位湾曲区間を前記第1の目標湾曲角度に湾曲した後に、前記第1の湾曲角度のプロファイルに従って前記追従湾曲区間が湾曲および前進移動し、前進移動の変位量が前記追従湾曲区間の長さに相当する第1の変位量に至る前に、前記遠位湾曲区間の目標湾曲角度が、前記第1の目標湾曲角度から第2の目標湾曲角度へ変更された場合、前記第1の湾曲角度のプロファイルとは異なる第2の湾曲角度のプロファイルが設定され、前記連続体ロボットのさらなる前進移動により前記第2の湾曲角度のプロファイルに従って前記追従湾曲区間の湾曲角度が前記第2の目標湾曲角度に至るよう構成されている。
また、本発明は、上述した連続体ロボットの制御方法、及び、上述した連続体ロボットとしてコンピュータを機能させるためのプログラムを含む。
また、本発明は、上述した連続体ロボットの制御方法、及び、上述した連続体ロボットとしてコンピュータを機能させるためのプログラムを含む。
本発明によれば、連続体ロボットの操作性の向上を図ることができる。
以下に、図面を参照しながら、本発明を実施するための形態(実施形態)について説明する。具体的な連続体ロボットは、基台部と、遠位用のワイヤが駆動することによって湾曲する遠位湾曲区間と、遠位湾曲区間と基台部との間に設けられ、追従用のワイヤが駆動することによって湾曲する追従湾曲区間と、を備えている。さらに、連続体ロボットは、遠位用のワイヤおよび追従用のワイヤを独立に駆動する駆動手段と、基台部、遠位湾曲区間、ならびに追従湾曲区間を一体にして前進移動させる移動手段と、制御部を備えている。この制御部は、入力された遠位湾曲区間の第1の目標湾曲角度に応じて設定される、連続体ロボットの前進移動に対応した追従湾曲区間の湾曲角度に関する第1の湾曲角度のプロファイルに従い、追従湾曲区間の湾曲角度が第1の目標湾曲角度に至るよう構成される。加えて、この制御部は、遠位湾曲区間を第1の目標湾曲角度に湾曲した後に、第1の湾曲角度のプロファイルに従って追従湾曲区間が湾曲および前進移動し、前進移動の移動量が追従湾曲区間の長さに相当する第1の移動量に至る前に、遠位湾曲区間の目標湾曲角度が、第1の目標湾曲角度から第2の目標湾曲角度へ変更された場合、以下のように制御される。すなわち、第1の湾曲角度のプロファイルとは異なる第2の湾曲角度のプロファイルが設定され、連続体ロボットのさらなる前進移動により、第2の湾曲角度のプロファイルに従って追従湾曲区間の湾曲角度が第2の目標湾曲角度に至るよう構成されている。
(第1の実施形態)
まず、本発明の第1の実施形態について説明する。
まず、本発明の第1の実施形態について説明する。
図1Aは、本発明の第1の実施形態に係る連続体ロボット100の概略構成の一例を示す図である。図1Aには、連続体ロボット100の一構成として、基台部140及び湾曲可能部170が図示されている。
湾曲可能部170は、ワイヤ(線状部材)が駆動することによって湾曲する複数の湾曲区間171〜173が設けられた構成部(湾曲区間が複数設けられた構成部)である。具体的に、図1Aには、(n−2)に係る湾曲区間171、(n−1)に係る湾曲区間172、nに係る湾曲区間173のn個の湾曲区間が図示されている。また、図1Aでは、湾曲区間173の湾曲角度をθnで示し、湾曲区間172の湾曲角度をθn-1で示し、湾曲区間171の湾曲角度をθn-2で示している。基台部140は、湾曲可能部170を支える構成部であり、その内部には、湾曲区間171〜173のそれぞれのワイヤを独立に駆動させるための駆動部(駆動手段)であるアクチュエータ(図1Aでは不図示)が設けられている。また、図1Aでは、基台部140の上面141の所定位置(例えば中心の位置)に原点Oをとり、連続体ロボット100の進行方向(例えば前進する方向)をz方向とし、このz方向と直交する方向であって相互に直交する方向をx方向及びy方向とした、xyz座標系を図示している。このように、連続体ロボット100は、湾曲可能部170を構成する複数の湾曲区間171〜173による湾曲動作に加えて、湾曲区間171〜173を含む湾曲可能部170と基台部140とが一体に、位置制御されたスライダ(移動手段)によりz方向に移動(前進移動)が可能となっている。この際、図1Aでは、連続体ロボット100のz方向の移動量(前進量、変位量)を示す指標として基台部140の変位zbを図示している。以下では、遠位湾曲区間として連続体ロボット100の先頭である最遠位湾曲区間を例に挙げ、また追従湾曲区間として最遠位湾曲区間に連なる後続湾曲区間を例に挙げて、説明することがある。
図1Aにおいて、湾曲区間173は、湾曲可能部170を構成する複数の湾曲区間171〜173のうち、基台部140から最も遠い位置にある最遠位湾曲区間である。湾曲区間172(湾曲区間171も同様)は、最遠位湾曲区間である湾曲区間173と基台部140との間に位置し、連続体ロボット100が前進する際に最遠位湾曲区間である湾曲区間173の後に続く後続湾曲区間である。なお、連続体ロボット100が前進する際に(図1Aのz方向の正の向き)、最遠位湾曲区間である湾曲区間173は、先頭の湾曲区間となる。
図1Bは、図1Aに示す連続体ロボット100の概略構成のうち、1つの湾曲区間171に関する詳細な概略構成の一例を示す図である。即ち、図1Bは、基台部140から最も近い位置にある近位端の湾曲区間171に関する詳細な概略構成を示している。この図1Bにおいて、図1Aに示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。なお、図1Bでは、湾曲区間171の湾曲角度をθ1で示し、また、湾曲区間171の旋回角度をζ1で示し、湾曲区間171の曲率半径(図1Bでは、点Oと点w1とを結ぶ線分に相当する)をρ1で示している。
連続体ロボット100は、湾曲区間171の遠位端160の接続部121〜123にワイヤ(線状部材)111〜113が接続され、このワイヤ111〜113が基台部140の内部に設置されているそれぞれのアクチュエータ131〜133で押し引きされることによって、姿勢(湾曲形状)が制御される。ここで、アクチュエータ131は、ワイヤ111を駆動させるための駆動部であり、また、アクチュエータ132は、ワイヤ112を駆動させるための駆動部であり、さらに、アクチュエータ133は、ワイヤ113を駆動させるための駆動部である。
また、連続体ロボット100は、湾曲区間171において、ワイヤ111〜113を案内するための部材であるワイヤガイド161〜164を有する。ワイヤガイド161〜164は、複数の部材を離散的に配置する方法の他に、蛇腹状やメッシュ状などの連続体部材を用いてもよい。ワイヤガイド161〜164は、ワイヤ111に、固定部150〜153において固定される。また、図1Bにおいて、連続体ロボット100の中心軸を破線で示している。
また、本実施形態では、ワイヤ111〜113を、xy平面内において反時計回りに、aワイヤ、bワイヤ及びcワイヤと呼ぶ。具体的に、図1Bに示す例では、ワイヤ111がaワイヤに相当し、湾曲区間171のアクチュエータ131の押し引きによるワイヤ111の駆動変位をlp1aとして図示している。また、図1Bに示す例では、ワイヤ112がbワイヤに相当し、湾曲区間171のアクチュエータ132の押し引きによるワイヤ112の駆動変位をlp1bとして図示している。さらに、図1Bに示す例では、ワイヤ113がcワイヤに相当し、湾曲区間171のアクチュエータ133の押し引きによるワイヤ113の駆動変位をlp1cとして図示している。
なお、図1Bに示す例では、湾曲区間171に関する詳細な概略構成のみを図示してその説明を行った。この点、図1Aに示す湾曲区間172及び湾曲区間173についても、図1Bに示す湾曲区間171に関する詳細な概略構成と同様に、それぞれが、ワイヤ(線状部材)111〜113に相当するワイヤや、アクチュエータ131〜133に相当するアクチュエータ、遠位端160に相当する遠位端、及び、ワイヤガイド161〜164に相当するワイヤガイドを有して構成されている。ここで、一般化して、第nの湾曲区間を駆動するワイヤの駆動変位をlpna,lpnb,lpncとする。
図2は、図1Bに示す3つのワイヤ111〜113(aワイヤ〜cワイヤ)のxy平面における配置例を示す図である。図2に示すように、図1Bに示す3つのワイヤ111〜113(aワイヤ〜cワイヤ)は、一片の長さをrsとする正三角形の頂点に配置されており、また、図2に示す位相角ξnは、第nの湾曲区間を駆動するワイヤの配置を決定する角度である。
図3は、本発明の第1の実施形態に係る連続体ロボットの制御システム10の概略構成の一例を示す図である。連続体ロボットの制御システム10は、図3に示すように、連続体ロボット100、連続体ロボット制御装置200(制御部)、及び、各種の入力装置310,330〜340を有して構成されている。なお、図3では、連続体ロボット100における外部の構成として、連続体ロボット制御装置200(制御部)及び入力装置310,330〜340が構成されている例を示しているが、本発明においては、この図3に示す形態に限定されるものではない。例えば、連続体ロボット100の内部の一構成として、連続体ロボット制御装置200(制御部)及び入力装置310,330〜340が構成されている形態も、本発明に含まれる。
入力装置310は、最遠位湾曲区間の目標湾曲角度θLを連続体ロボット制御装置200に入力する装置である。具体的に、図1Aに示す例では、入力装置310は、最遠位湾曲区間である湾曲区間173の目標湾曲角度θnを、最遠位湾曲区間の目標湾曲角度θLとして、連続体ロボット制御装置200の後続湾曲角度算出部210及び運動学演算部220に入力する。なお、入力装置310は、最遠位湾曲区間の目標湾曲角度θLに加えて、最遠位湾曲区間の目標旋回角度ζLを更に連続体ロボット制御装置200に入力するようにしてもよい。
入力装置330は、連続体ロボット100の進行方向(例えば前進する方向)における基台部140の変位zbを連続体ロボット制御装置200に入力するとともに、連続体ロボット100の基台部140を変位zbだけz方向に移動させるべく制御を行う装置である。この制御により、基台部140および複数の湾曲区間171〜173を含む湾曲可能部170を一体にして前進移動させることができる。
入力装置340は、各種の情報を連続体ロボット制御装置200に入力する装置である。具体的に、入力装置340は、例えば、後続湾曲区間である湾曲区間172の長さl(湾曲区間171の長さを含めてもよい)の情報や、使用する参照テーブル2111の指示情報等を、連続体ロボット制御装置200の情報入力部213に入力する。
連続体ロボット制御装置200は、図3に示すように、後続湾曲角度算出部210、及び、運動学演算部220を有して構成されている。
後続湾曲角度算出部210は、入力装置310から入力された最遠位湾曲区間の目標湾曲角度θLと、後続湾曲区間の目標湾曲角度θFと、入力装置330から入力された基台部140の変位zbと、入力装置340から入力された後続湾曲区間の長さlとに基づいて、最遠位湾曲区間の目標湾曲角度θLの変更に伴う変更後の後続湾曲区間の目標湾曲角度θF'を算出する算出手段である。この後続湾曲角度算出部210は、図3に示すように、マルチプレクサMux、記憶部211、参照テーブル書換部212、及び、情報入力部213を有して構成されている。記憶部211は、最遠位湾曲区間の目標湾曲角度θL及び後続湾曲区間の目標湾曲角度θFと基台部140の変位zbとの関係を示す異なる複数の参照テーブル2111や、後続湾曲角度算出部210において処理に必要な各種の情報を記憶している記憶手段である。情報入力部213は、マルチプレクサMuxを介して入力装置340から入力された後続湾曲区間の長さlの情報や使用する参照テーブル2111の指示情報等を、参照テーブル書換部212に対して入力する情報入力手段である。参照テーブル書換部212は、情報入力部213から入力された情報に基づき、記憶部211に記憶されている複数の参照テーブル2111の中から使用する1つの参照テーブル2111を選択し、最遠位湾曲区間の目標湾曲角度θLの変更に応じて当該選択した参照テーブル2111を書き換える書換手段である。なお、ここで説明した後続湾曲角度算出部210の詳細な処理例については、図7等を用いて後述する。
運動学演算部220は、後続湾曲角度算出部210で算出された変更後の後続湾曲区間の目標湾曲角度θF'に基づいて、駆動部である各アクチュエータで後続湾曲区間のワイヤ(線状部材)を駆動させる際の駆動変位を演算する演算手段である。また、運動学演算部220は、入力装置310から入力された最遠位湾曲区間の目標湾曲角度θLに基づいて、駆動部である各アクチュエータで最遠位湾曲区間のワイヤ(線状部材)を駆動させる際の駆動変位を演算する。図3では、運動学演算部220の演算により得られた最遠位湾曲区間のワイヤの駆動変位と後続湾曲区間のワイヤの駆動変位をまとめて、ワイヤの駆動変位lpとして図示している。そして、本実施形態に係る連続体ロボット100は、それぞれのワイヤの駆動変位lpに応じて、最遠位湾曲区間及び後続湾曲区間の湾曲制御が行われることになる。
本実施形態では、図2に示す、全ての位相角ξn=0とし、以下には、まず、xz平面における先頭追従制御について説明する。
1)モデリング
本章では、連続体ロボット100のxz平面における運動学を導出する。
本章で用いる記号の定義を以下に示す。
ln:第nの湾曲区間のアーム筐体の長さ
rn:xz平面におけるaワイヤとbワイヤの距離
e:連続体ロボット100の湾曲可能部170の湾曲区間数
θn:第nの湾曲区間の(遠位端の)湾曲角度
ρn:第nの湾曲区間の曲率半径
θrefn:第nの湾曲区間の(遠位端の)目標湾曲角度
lpn:第nの湾曲区間のワイヤの駆動変位
xtn,ztn:第nの湾曲区間の遠位端の座標
zb:基台部140の変位
本章では、連続体ロボット100のxz平面における運動学を導出する。
本章で用いる記号の定義を以下に示す。
ln:第nの湾曲区間のアーム筐体の長さ
rn:xz平面におけるaワイヤとbワイヤの距離
e:連続体ロボット100の湾曲可能部170の湾曲区間数
θn:第nの湾曲区間の(遠位端の)湾曲角度
ρn:第nの湾曲区間の曲率半径
θrefn:第nの湾曲区間の(遠位端の)目標湾曲角度
lpn:第nの湾曲区間のワイヤの駆動変位
xtn,ztn:第nの湾曲区間の遠位端の座標
zb:基台部140の変位
図4及び図5は、本発明の第1の実施形態を示し、図1Aに示す連続体ロボット100の運動学モデルの一例を示す図である。
ここでは、以下の仮定をおき、図4に示す湾曲区間数をnとする連続体ロボット100の運動学を導出する。
1.ワイヤは、xz平面内のみに変形する。
2.各湾曲区間において、ワイヤは曲率一定に変形する。
3.ワイヤのねじり変形は考慮しない。
4.ワイヤは、長手方向に変形しない。
ここでは、以下の仮定をおき、図4に示す湾曲区間数をnとする連続体ロボット100の運動学を導出する。
1.ワイヤは、xz平面内のみに変形する。
2.各湾曲区間において、ワイヤは曲率一定に変形する。
3.ワイヤのねじり変形は考慮しない。
4.ワイヤは、長手方向に変形しない。
まず、第1の湾曲区間(図1A及び図1Bの湾曲区間171に相当)のみを考える。
aワイヤを駆動し、bワイヤ及びcワイヤを固定するとき、ワイヤの駆動変位lp1と第1の湾曲区間の(遠位端の)湾曲角度θ1との関係は、以下の(1)式となる。
aワイヤを駆動し、bワイヤ及びcワイヤを固定するとき、ワイヤの駆動変位lp1と第1の湾曲区間の(遠位端の)湾曲角度θ1との関係は、以下の(1)式となる。
次に、第nの湾曲区間におけるワイヤの駆動変位lpnと、その遠位端の湾曲角度θnとの関係を導出する。ただし、ここでは、nは、2以上とする。第nの湾曲区間の(遠位端の)相対湾曲角度θ~ nを以下のように定義する。
θ~ n=θn−θn-1 ・・・(2)
θ~ n=θn−θn-1 ・・・(2)
そして、図5に示すように、原点Oの座標を(xtn-1,ztn-1)とし、θn-1方向及びその直交方向からなる相対座標系xn-znをとると、相対座標系xn-znにおけるワイヤの駆動変位l~ pnと第nの湾曲区間の(遠位端の)相対湾曲角度θ~ nとの関係は、以下の(3)式となる。ただし、以下の(3)式では、l~ pnの「〜」はlの上方に記載し、また、θ~ nの「〜」はθの上方に記載している。
これにより、第nの湾曲区間の遠位端の湾曲角度θnは、ワイヤの駆動変位lpnによってのみ決まり、途中の湾曲区間の角度には依存しないことがわかる。
次に、第n湾曲区間の遠位端の湾曲角度とその遠位端の座標との関係を導出する。まず、第1の湾曲区間を考えると、その第1の湾曲区間の遠位端の湾曲角度θ1とその遠位端の座標(xt1,zt1)との関係は、以下の(5)式及び(6)式となる。
次に、第nの湾曲区間の遠位端の相対湾曲角度θ~ nと相対座標系xn-znにおける遠位端の座標(x~ tn、z~ tn)との関係は、以下の(7)式〜(9)式となる。ここで、nは、2以上である。ただし、以下の(7)式及び(8)式では、θ~ nの「〜」はθの上方に記載し、また、x~ tn及びz~ tnの「〜」はそれぞれx及びzの上方に記載している。
これにより、絶対座標系での遠位端の座標(xtn,ztn)は、回転変換行列を用いて表される。
2)制御系設計
本章では、先頭追従制御系の設計を行う。
図6は、本発明の第1の実施形態に係る連続体ロボット100の先頭追従制御の一例を示す図である。この図6において、図1Aに示す構成と同様の構成については同じ符号を付している。また、図6では、図1Aに示すz方向を紙面下側から紙面上側に向かう方向にとっている。また、図6には、基台部140及び湾曲可能部170を含み構成された連続体ロボット100が進む目標経路610を点線で図示している。
本章では、先頭追従制御系の設計を行う。
図6は、本発明の第1の実施形態に係る連続体ロボット100の先頭追従制御の一例を示す図である。この図6において、図1Aに示す構成と同様の構成については同じ符号を付している。また、図6では、図1Aに示すz方向を紙面下側から紙面上側に向かう方向にとっている。また、図6には、基台部140及び湾曲可能部170を含み構成された連続体ロボット100が進む目標経路610を点線で図示している。
ここで、先頭追従制御とは、図6に示すように、湾曲可能部170の最遠位湾曲区間が通る経路(目標経路610)と同じ経路を後続の湾曲区間が通るように制御する方法である。先頭追従制御の一例としては、入力された遠位湾曲区間の第1の目標湾曲角度に応じて設定される、連続体ロボット100の前進移動に対応した追従湾曲区間の湾曲角度に関する第1の湾曲角度のプロファイルに従って、追従湾曲区間の湾曲角度が第1の目標湾曲角度に至るよう構成されている制御がある。
図6において、時点601は、基台部140の上面からz方向に延びる湾曲可能部170が湾曲していない初期状態を示している。その後、図6においては、時点602,時点603,時点604,時点605と時間が経過するのにしたがって、基台部140がz方向に移動して湾曲可能部170が湾曲する様子が示されている。
このような先頭追従制御により、連続体ロボット100は、空間をすり抜けるように進むことができる。なお、先頭追従制御は、予め目標経路610が定められていることは必須ではなく、例えば、最遠位湾曲区間の湾曲角度を、後続の湾曲区間の湾曲角度に当該湾曲区間の長さをもって連続的に伝播させてもよい。この方法を用いると、例えば、操縦者は、ジョイステックなどで最遠位湾曲区間の湾曲角度と基台部140の変位(移動量(前進量))に対してのみ指令を与えることで、連続体ロボット100を実時間で先頭追従制御することができる。
図7は、本発明の第1の実施形態を示し、図3の後続湾曲角度算出部210による最遠位湾曲区間の目標湾曲角度θLの変更に伴う変更後の後続湾曲区間の目標湾曲角度θF'の算出処理の一例を示す図である。図7において、グラフ上の座標は、基台部140の位置と湾曲区間(湾曲セクション)の湾曲角度との対を示している。説明のため、以下では、座標aに対応する湾曲角度のことを"角度a"、そして座標aに対応する基台部140の位置(変位)を位置a(変位a)と呼ぶことがある。なお、以下の図7の説明では、最遠位湾曲区間として図1Aの湾曲区間173を適用し、後続湾曲区間として図1Aの湾曲区間172を適用した場合を想定した例について説明する。
図7(a)において、横軸は、基台部140の変位zbを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の湾曲角度θを示している。図7(a)において、破線の「Leader」は、操縦者等により入力装置310を介して入力された最遠位湾曲区間の目標湾曲角度θLに対応する、最遠位湾曲区間の湾曲角度のプロファイルを示している。また、図7(a)において、太破線の「Follower」は、参照テーブル書換部212による書き換えが行われる前の後続湾曲区間の目標湾曲角度θFに対応する、追従湾曲区間の湾曲角度のプロファイルを示している。
具体的には、基台部140の変位zbが変位aにおいて、最遠位湾曲区間の目標湾曲角度θLが最遠位湾曲角度aから最遠位湾曲角度bに変更されると、太破線の「Follower」で示す後続湾曲区間の目標湾曲角度θFは、基台部140の変位zbが変位cにおいて、後続湾曲角度cから後続湾曲角度dに変更されるように自動生成される。ここで、変位cは、変位aと変位cの間の長さが後続湾曲区間の長さlとなるように決定される。
しかしながら、図7(a)において太破線の「Follower」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルでは、基台部140の変位zbが変位aと変位cとの間にあるときには、後続湾曲区間の目標湾曲角度θFは変化しない。そして、図7(a)において太破線の「Follower」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルでは、変位cにおいて後続湾曲角度cから後続湾曲角度dとなるため、連続体ロボット100は急峻な挙動を示し、連続体ロボット100の操作性が損なわれる。そこで、本発明の第1の実施形態では、図7(a)において実線の「Interpolated follower」で示す変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルとして、変位aと変位cとの間では、目標湾曲角度cと目標湾曲角度dと直線で結ぶように補間する。
具体的に、後続湾曲角度算出部210の参照テーブル書換部212は、以下のようにして、図7(a)に示す参照テーブルにおいて太破線の「Follower」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルを、実線の「Interpolated follower」で示す変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルに書き換える。
まず、参照テーブル書換部212は、図7(a)に示す参照テーブルにおいて、最遠位湾曲区間の目標湾曲角度θLが第1の最遠位湾曲角度aから第2の最遠位湾曲角度bに変更されると、第2の最遠位湾曲角度bとなった際の基台部140の変位である第1変位aから、第1変位aから後続湾曲区間の長さlだけ進んだ変位である第2変位cまでを底辺の長さとし(第1変位aと長さlの和)、第2の最遠位湾曲角度bと第1の最遠位湾曲角度aとの差を高さとする直角三角形を生成する。具体的には、図7(a)で斜線のハッチングを付した直角三角形、即ち、第1変位aから第2変位cまでを底辺の長さとし、第2変位cにおける目標湾曲角度dと目標湾曲角度cとの差を高さとする直角三角形を生成する。
続いて、参照テーブル書換部212は、生成した直角三角形の斜辺上の点から直角三角形の底辺までの目標湾曲角度に係る値(具体的には、第1変位aから第2変位cまでの各変位において、基台部140の変位zbの座標軸(横軸)と直交する縦の直線と直角三角形の斜辺との交点を求め、その交点から直角三角形の底辺までの長さの値)を、図7(a)において太破線の「Follower」で示す後続湾曲区間の目標湾曲角度θFに係る値(図7(a)に示す例では、0)に加算し、当該加算後の後続湾曲区間の目標湾曲角度を、実線の「Interpolated follower」で示す変更後の後続湾曲区間の目標湾曲角度θF'として、図7(a)に示す参照テーブルを書き換える。その後、参照テーブル書換部212は、書き換えた参照テーブルを記憶部211に記憶する。
まず、参照テーブル書換部212は、図7(a)に示す参照テーブルにおいて、最遠位湾曲区間の目標湾曲角度θLが第1の最遠位湾曲角度aから第2の最遠位湾曲角度bに変更されると、第2の最遠位湾曲角度bとなった際の基台部140の変位である第1変位aから、第1変位aから後続湾曲区間の長さlだけ進んだ変位である第2変位cまでを底辺の長さとし(第1変位aと長さlの和)、第2の最遠位湾曲角度bと第1の最遠位湾曲角度aとの差を高さとする直角三角形を生成する。具体的には、図7(a)で斜線のハッチングを付した直角三角形、即ち、第1変位aから第2変位cまでを底辺の長さとし、第2変位cにおける目標湾曲角度dと目標湾曲角度cとの差を高さとする直角三角形を生成する。
続いて、参照テーブル書換部212は、生成した直角三角形の斜辺上の点から直角三角形の底辺までの目標湾曲角度に係る値(具体的には、第1変位aから第2変位cまでの各変位において、基台部140の変位zbの座標軸(横軸)と直交する縦の直線と直角三角形の斜辺との交点を求め、その交点から直角三角形の底辺までの長さの値)を、図7(a)において太破線の「Follower」で示す後続湾曲区間の目標湾曲角度θFに係る値(図7(a)に示す例では、0)に加算し、当該加算後の後続湾曲区間の目標湾曲角度を、実線の「Interpolated follower」で示す変更後の後続湾曲区間の目標湾曲角度θF'として、図7(a)に示す参照テーブルを書き換える。その後、参照テーブル書換部212は、書き換えた参照テーブルを記憶部211に記憶する。
図7(a)では、最遠位湾曲区間の目標湾曲角度θLにおける一度の変更操作指令に対する後続湾曲区間の目標湾曲角度θFの変更処理方法について図示している。本実施形態では、最遠位湾曲区間の目標湾曲角度θLにおける更なる変更操作指令(複数回の変更操作指令)に対しても、同様のアルゴリズムを用いて後続湾曲区間の目標湾曲角度θFの変更処理を行うことができる。
図7(b)は、図7(a)に示す最遠位湾曲区間の目標湾曲角度θLにおける1度目の変更操作指令がなされた後に2度目の変更操作指令がなされた場合の例を示している。
図7(b)において、図7(a)と同様に、横軸は、基台部140の変位zbを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の湾曲角度θを示している。図7(b)において、破線の「Leader」は、操縦者等により入力装置310を介して入力された最遠位湾曲区間の目標湾曲角度θLに係る湾曲角度のプロファイルを示している。また、図7(b)においては、図7(a)において実線の「Interpolated follower」で示す1回目の変更後の後続湾曲区間の目標湾曲角度θF'を、太い一点鎖線の「First Interpolation」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルとして示している。この図7(b)に示す例は、後続湾曲区間の長さlに満たない基台部140の変位zb(変位aから変位cまでの範囲内の変位)の際に、最遠位湾曲区間の目標湾曲角度θLが最遠位湾曲角度bから最遠位湾曲角度b'に変更された場合を示している。
参照テーブル書換部212は、図7(b)に示す参照テーブルにおいて、最遠位湾曲区間の目標湾曲角度θLが第1の最遠位湾曲角度bから第2の最遠位湾曲角度b'に変更されると、第2の最遠位湾曲角度b'となった際の基台部140の変位である第1変位a'から、後続湾曲区間の長さlの和となる変位である第2変位c'までを底辺の長さとし、第2の最遠位湾曲角度b'と第1の最遠位湾曲角度bとの差を高さとする直角三角形を生成する。具体的には、図7(b)で斜線のハッチングを付した直角三角形、即ち、第1変位a'から第2変位c'までを底辺の長さとし、第2変位c'における目標湾曲角度d'と目標湾曲角度c'との差を高さとする直角三角形を生成する。
続いて、参照テーブル書換部212は、生成した直角三角形の斜辺上の点から直角三角形の底辺までの目標湾曲角度に係る値(具体的には、第1変位a'から第2変位c'までの各変位において、基台部140の変位zbの座標軸(横軸)と直交する縦の直線と直角三角形の斜辺との交点を求め、その交点から直角三角形の底辺までの長さの値)を、図7(b)において太い一点鎖線の「First Interpolation」で示す後続湾曲区間の目標湾曲角度θFに係る値に加算し、当該加算後の後続湾曲区間の目標湾曲角度を、実線の「Interpolated follower」で示す変更後の後続湾曲区間の目標湾曲角度θF'として、図7(b)に示す参照テーブルを書き換える。その後、参照テーブル書換部212は、書き換えた参照テーブルを記憶部211に記憶する。
続いて、参照テーブル書換部212は、生成した直角三角形の斜辺上の点から直角三角形の底辺までの目標湾曲角度に係る値(具体的には、第1変位a'から第2変位c'までの各変位において、基台部140の変位zbの座標軸(横軸)と直交する縦の直線と直角三角形の斜辺との交点を求め、その交点から直角三角形の底辺までの長さの値)を、図7(b)において太い一点鎖線の「First Interpolation」で示す後続湾曲区間の目標湾曲角度θFに係る値に加算し、当該加算後の後続湾曲区間の目標湾曲角度を、実線の「Interpolated follower」で示す変更後の後続湾曲区間の目標湾曲角度θF'として、図7(b)に示す参照テーブルを書き換える。その後、参照テーブル書換部212は、書き換えた参照テーブルを記憶部211に記憶する。
図7(b)に示すように、最遠位湾曲区間の目標湾曲角度θLにおける2度目の変更操作指令が後続湾曲区間の長さlに満たない変位aから変位cまでの範囲内において行われると、直線efの傾きは、直線adの傾きと直線a'd'の傾きとの和となる。これは、後続湾曲区間の長さlを超えない基台部140の変位zbにおいて、変更操作指令が連続することは操作の修正であると考えると、本実施形態における変更後の後続湾曲区間の目標湾曲角度θF'の算出方法は、以下の特徴を持つ。
・ 最遠位湾曲区間の目標湾曲角度θLにおける1度目の変更操作指令と2度目の変更操作指令が同方向の場合には、後続湾曲区間は、点a'から点dまでの変位において、湾曲角度の角速度が増加し、2度目の変更操作指令が強調される湾曲形状に制御される。
・ 最遠位湾曲区間の目標湾曲角度θLにおける1度目の変更操作指令と2度目の変更操作指令が逆方向の場合には、後続湾曲区間は、点a'から点dまでの変位において、湾曲角度の角速度が減少し、1度目の変更操作指令が軽減される湾曲形状に制御される。
以上のように、追従湾曲区間(後続湾曲区間)は、第1の湾曲角度のプロファイルとは異なる第2の湾曲角度のプロファイルに従って、点d'に至るように制御される。
このようにして、第1の湾曲角度のプロファイルとは異なる第2の湾曲角度のプロファイルが設定される。そして、第1の湾曲角度のプロファイルに替えて、第2の湾曲角度のプロファイルに従って追従湾曲区間(後続湾曲区間)の湾曲角度が第2の目標湾曲角度に至るよう構成されている。湾曲角度が目標湾曲角度に至ったかどうかは、使用者が所望の判定基準を設けて判定するとよい。
・ 最遠位湾曲区間の目標湾曲角度θLにおける1度目の変更操作指令と2度目の変更操作指令が同方向の場合には、後続湾曲区間は、点a'から点dまでの変位において、湾曲角度の角速度が増加し、2度目の変更操作指令が強調される湾曲形状に制御される。
・ 最遠位湾曲区間の目標湾曲角度θLにおける1度目の変更操作指令と2度目の変更操作指令が逆方向の場合には、後続湾曲区間は、点a'から点dまでの変位において、湾曲角度の角速度が減少し、1度目の変更操作指令が軽減される湾曲形状に制御される。
以上のように、追従湾曲区間(後続湾曲区間)は、第1の湾曲角度のプロファイルとは異なる第2の湾曲角度のプロファイルに従って、点d'に至るように制御される。
このようにして、第1の湾曲角度のプロファイルとは異なる第2の湾曲角度のプロファイルが設定される。そして、第1の湾曲角度のプロファイルに替えて、第2の湾曲角度のプロファイルに従って追従湾曲区間(後続湾曲区間)の湾曲角度が第2の目標湾曲角度に至るよう構成されている。湾曲角度が目標湾曲角度に至ったかどうかは、使用者が所望の判定基準を設けて判定するとよい。
上述した図7の説明では、最遠位湾曲区間として図1Aの湾曲区間173を適用し、後続湾曲区間として図1Aの湾曲区間172を適用した場合を想定した例について説明した。そして、連続体ロボット100の湾曲可能部170の湾曲区間数が3つ以上、例えば、図1Aに示す湾曲可能部170の湾曲区間数が3つの場合において、上述した後続湾曲区間として図1Aの湾曲区間171を適用する場合には、例えば、上述した最遠位湾曲区間として1つ前の図1Aの湾曲区間172を適用することで実現可能である。
図8は、本発明の第1の実施形態に係る連続体ロボットの制御システム10による連続体ロボットの制御方法における処理手順の一例を示すフローチャートである。
まず、ステップS801において、連続体ロボット制御装置200は、入力装置310から、基台部140から最遠位端に位置する湾曲区間である最遠位湾曲区間に対する操作入力を受け付ける。このステップS801では、連続体ロボット制御装置200は、入力装置310から、例えば最遠位湾曲区間の目標湾曲角度θLの入力を受け付ける。
続いて、ステップS802において、連続体ロボット制御装置200は、運動学演算部220において、入力された最遠位湾曲区間の目標湾曲角度θLに基づき最遠位湾曲区間のワイヤ(線状部材)の駆動変位を演算し、当該演算結果に基づき連続体ロボット100を制御する。この制御により、連続体ロボット100の湾曲可能部170の湾曲形状が変更される。
続いて、ステップS803において、連続体ロボット制御装置200は、例えば各入力装置310,330〜340からの入力情報に基づいて、連続体ロボット100に対する操作入力が終了したか否か判断する。
ステップS803の判断の結果、連続体ロボット100に対する操作入力が終了していない場合には(S803/No)、ステップS804に進む。ステップS804に進むと、連続体ロボット制御装置200は、例えば入力装置330からの入力情報に基づいて、連続体ロボット100の基台部140が前進したか否かを判断する。このステップS804の判断の結果、連続体ロボット100の基台部140が前進していない場合には(S804/No)、ステップS801に戻り、ステップS801以降の処理を再度行う。
一方、ステップS804の判断の結果、連続体ロボット100の基台部140が前進した場合には(S804/Yes)、ステップS805に進む。ステップS805に進むと、後続湾曲角度算出部210の参照テーブル書換部212は、図7(a)または図7(b)に示す参照テーブルにおいて、斜線のハッチングを付した直角三角形データを生成する。図7(a)の場合、第1変位aから第2変位cまでの後続湾曲区間の長さlを底辺の長さ(幅)とし、第2の最遠位湾曲角度θL2に相当する目標湾曲角度bと第1の最遠位湾曲角度θL1に相当する目標湾曲角度aとの差を高さ(より具体的には、第2変位cにおける目標湾曲角度dと目標湾曲角度cとの差を高さ)とする直角三角形を生成する。図7(b)の場合、第1変位a'から第2変位c'までの後続湾曲区間の長さlを底辺の長さ(幅)とし、第2の最遠位湾曲角度θL2に相当する目標湾曲角度b'と第1の最遠位湾曲角度θL1に相当する目標湾曲角度bとの差を高さ(より具体的には、第2変位c'における目標湾曲角度d'と目標湾曲角度c'との差を高さ)とする直角三角形を生成する。
続いて、ステップS806において、後続湾曲角度算出部210の参照テーブル書換部212は、ステップS805で生成した直角三角形データを、記憶部211に記憶されている現在の参照テーブルの後続湾曲区間の目標湾曲角度θFに加算して補間する。この補間により得られる変更後の後続湾曲区間の目標湾曲角度θF'は、図7(a)または図7(b)に示す参照テーブルにおいて実線で示されている。
続いて、ステップS807において、後続湾曲角度算出部210の参照テーブル書換部212は、最遠位湾曲区間の目標湾曲角度θLについて、図7(a)であれば変位c以降(変位cよりも右側)のデータ、図7(b)であれば変位c'以降(変位c'よりも右側)のデータを書き込む処理を行う。このステップS807の処理が終了すると、ステップS801に戻る。
また、ステップS803の判断の結果、連続体ロボット100に対する操作入力が終了した場合には(S803/Yes)、図8に示すフローチャートの処理を終了する。
まず、ステップS801において、連続体ロボット制御装置200は、入力装置310から、基台部140から最遠位端に位置する湾曲区間である最遠位湾曲区間に対する操作入力を受け付ける。このステップS801では、連続体ロボット制御装置200は、入力装置310から、例えば最遠位湾曲区間の目標湾曲角度θLの入力を受け付ける。
続いて、ステップS802において、連続体ロボット制御装置200は、運動学演算部220において、入力された最遠位湾曲区間の目標湾曲角度θLに基づき最遠位湾曲区間のワイヤ(線状部材)の駆動変位を演算し、当該演算結果に基づき連続体ロボット100を制御する。この制御により、連続体ロボット100の湾曲可能部170の湾曲形状が変更される。
続いて、ステップS803において、連続体ロボット制御装置200は、例えば各入力装置310,330〜340からの入力情報に基づいて、連続体ロボット100に対する操作入力が終了したか否か判断する。
ステップS803の判断の結果、連続体ロボット100に対する操作入力が終了していない場合には(S803/No)、ステップS804に進む。ステップS804に進むと、連続体ロボット制御装置200は、例えば入力装置330からの入力情報に基づいて、連続体ロボット100の基台部140が前進したか否かを判断する。このステップS804の判断の結果、連続体ロボット100の基台部140が前進していない場合には(S804/No)、ステップS801に戻り、ステップS801以降の処理を再度行う。
一方、ステップS804の判断の結果、連続体ロボット100の基台部140が前進した場合には(S804/Yes)、ステップS805に進む。ステップS805に進むと、後続湾曲角度算出部210の参照テーブル書換部212は、図7(a)または図7(b)に示す参照テーブルにおいて、斜線のハッチングを付した直角三角形データを生成する。図7(a)の場合、第1変位aから第2変位cまでの後続湾曲区間の長さlを底辺の長さ(幅)とし、第2の最遠位湾曲角度θL2に相当する目標湾曲角度bと第1の最遠位湾曲角度θL1に相当する目標湾曲角度aとの差を高さ(より具体的には、第2変位cにおける目標湾曲角度dと目標湾曲角度cとの差を高さ)とする直角三角形を生成する。図7(b)の場合、第1変位a'から第2変位c'までの後続湾曲区間の長さlを底辺の長さ(幅)とし、第2の最遠位湾曲角度θL2に相当する目標湾曲角度b'と第1の最遠位湾曲角度θL1に相当する目標湾曲角度bとの差を高さ(より具体的には、第2変位c'における目標湾曲角度d'と目標湾曲角度c'との差を高さ)とする直角三角形を生成する。
続いて、ステップS806において、後続湾曲角度算出部210の参照テーブル書換部212は、ステップS805で生成した直角三角形データを、記憶部211に記憶されている現在の参照テーブルの後続湾曲区間の目標湾曲角度θFに加算して補間する。この補間により得られる変更後の後続湾曲区間の目標湾曲角度θF'は、図7(a)または図7(b)に示す参照テーブルにおいて実線で示されている。
続いて、ステップS807において、後続湾曲角度算出部210の参照テーブル書換部212は、最遠位湾曲区間の目標湾曲角度θLについて、図7(a)であれば変位c以降(変位cよりも右側)のデータ、図7(b)であれば変位c'以降(変位c'よりも右側)のデータを書き込む処理を行う。このステップS807の処理が終了すると、ステップS801に戻る。
また、ステップS803の判断の結果、連続体ロボット100に対する操作入力が終了した場合には(S803/Yes)、図8に示すフローチャートの処理を終了する。
3)シミュレーション
本章では、前章の2)制御系設計で説明した先頭追従制御系を用いてシミュレーションを行う。シミュレーションでは、湾曲区間数が2であり、第1の湾曲区間の長さを0.02mとし、第2の湾曲区間の長さを0.02mとした連続体ロボット100を対象とする。この際、例えば、第2の湾曲区間を上述した最遠位湾曲区間として適用し、第1の湾曲区間を上述した後続湾曲区間として適用する。
本章では、前章の2)制御系設計で説明した先頭追従制御系を用いてシミュレーションを行う。シミュレーションでは、湾曲区間数が2であり、第1の湾曲区間の長さを0.02mとし、第2の湾曲区間の長さを0.02mとした連続体ロボット100を対象とする。この際、例えば、第2の湾曲区間を上述した最遠位湾曲区間として適用し、第1の湾曲区間を上述した後続湾曲区間として適用する。
図9は、本発明の第1の実施形態を示し、最遠位湾曲区間の目標湾曲角度θLにおける1度目の変更操作指令と2度目の変更操作指令が同方向の場合の後続湾曲区間の目標湾曲角度におけるシミュレーション結果の一例を示す図である。この図9において、横軸は、基台部140の変位zbを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の湾曲角度θを示している。また、図9では、破線の「Leader」は、最遠位湾曲区間の目標湾曲角度θLに係る湾曲角度のプロファイルを示し、実線の「Follower」は、最遠位湾曲区間の目標湾曲角度θLの変更に伴う変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルを示している。
この図9では、連続体ロボット100の動作開始後にz方向に直進状態で0.02m前進させ、そこで、最遠位湾曲区間を45度湾曲させる1度目の湾曲変更操作指令がされている。その後、z方向に0.01m前進させ、そこで、最遠位湾曲区間を同方向にさらに15度(最終的に60度)湾曲させる2度目の湾曲変更操作指令がされている。
図10は、本発明の第1の実施形態を示し、図9に示すシミュレーション結果を反映した連続体ロボット100の湾曲可能部170における動作状態の一例を示す図である。具体的に、図10(a)〜図10(d)は、時系列ごとに連続体ロボット100の湾曲可能部170における動作状態を示したものであり、横軸はx方向の変位を示し、縦軸はz方向の変位を示している。また、図10(a)〜図10(d)において、実線は、連続体ロボット100の湾曲可能部170の湾曲状態を示し、破線は、目標経路610を示している。
図10(a)では、連続体ロボット100をz方向に直進状態で0.02mまで前進させ、そこで、湾曲可能部170の最遠位湾曲区間を45度に湾曲させている。続いて、図10(b)では、目標経路610との誤差を修正するために、連続体ロボット100の前進をやめ、続く、図10(c)において、湾曲可能部170の最遠位湾曲区間を60度まで湾曲させている。この2度目の湾曲操作は、1度目の湾曲操作から、湾曲区間の長さ(後続湾曲区間の長さl=0.02m)を超えない基台部140の変位Zbにおいて行われている。そのため、図9に示すように、実線の「Follower」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルは、基台部140の変位zbが0.03mから0.04mまでの間は角速度が大きく、湾曲操作の修正がその後の短い基台部140の変位zbにおいて後続湾曲区間に反映されるようになっている。これにより、図10(d)において、湾曲可能部170を目標経路610に接近させることができる。
図11は、本発明の第1の実施形態を示し、最遠位湾曲区間の目標湾曲角度θLにおける1度目の変更操作指令と2度目の変更操作指令が逆方向の場合の後続湾曲区間の目標湾曲角度におけるシミュレーション結果の一例を示す図である。この図11において、図9で説明した事項と同様の事項については説明を省略する。
この図11では、連続体ロボット100の動作開始後にz方向に直進状態で0.02m前進させ、そこで、最遠位湾曲区間を45度湾曲させる1度目の湾曲変更操作指令がされている。その後、z方向に0.01m前進させ、そこで、最遠位湾曲区間を逆方向に20度(最終的に25度)湾曲させる2度目の湾曲変更操作指令がされている。
図12は、本発明の第1の実施形態を示し、図11に示すシミュレーション結果を反映した連続体ロボット100の湾曲可能部170における動作状態の一例を示す図である。具体的に、図12(a)〜図12(d)は、時系列ごとに連続体ロボット100の湾曲可能部170における動作状態を示したものであり、横軸はx方向の変位を示し、縦軸はz方向の変位を示している。また、図10(a)〜図10(d)において、実線は、連続体ロボット100の湾曲可能部170の湾曲状態を示し、破線は、目標経路610を示している。
図12(a)では、連続体ロボット100をz方向に直進状態で0.02mまで前進させ、そこで、湾曲可能部170の最遠位湾曲区間を45度に湾曲させている。続いて、図12(b)では、目標経路610との誤差を修正するために、連続体ロボット100の前進をやめ、図12(c)において、湾曲可能部170の最遠位湾曲区間を25度まで湾曲を浅くしている。そのため、図11に示すように、実線の「Follower」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルは、基台部140の変位zbが0.03mから0.04mまでの間は角速度が小さく、後続湾曲区間の目標湾曲角度θFが45度に到達することは取り消され、湾曲操作の修正がその後の短い基台部140の変位zbにおいて後続湾曲区間に反映されるようになっている。これにより、図12(d)において、湾曲可能部170を目標経路610に接近させることができる。
以上説明した第1の実施形態に係る連続体ロボットの制御システム10では、図7等に示すように、最遠位湾曲区間の目標湾曲角度θLと、基台部140の変位zbと、後続湾曲区間の長さlとに基づいて、最遠位湾曲区間の目標湾曲角度θLの変更に伴う変更後の後続湾曲区間の目標湾曲角度θF'を算出するようにしている。
かかる構成によれば、湾曲区間(本実施形態では、後続湾曲区間)の長さに満たない基台部140の変位zbの際に最遠位湾曲区間の目標湾曲角度θLが変更された場合においても、連続体ロボット100の操作性の向上を図ることができる。これにより、連続体ロボット100を目標経路610に沿って動作させる対象物を傷つけたり、また、連続体ロボット100を破損させたりする不具合を抑制、あるいは回避することが可能となる。
かかる構成によれば、湾曲区間(本実施形態では、後続湾曲区間)の長さに満たない基台部140の変位zbの際に最遠位湾曲区間の目標湾曲角度θLが変更された場合においても、連続体ロボット100の操作性の向上を図ることができる。これにより、連続体ロボット100を目標経路610に沿って動作させる対象物を傷つけたり、また、連続体ロボット100を破損させたりする不具合を抑制、あるいは回避することが可能となる。
(第2の実施形態)
次に、本発明の第2の実施形態について説明する。なお、以下に記載する第2の実施形態の説明では、上述した第1の実施形態と共通する事項については説明を省略し、上述した第1の実施形態と異なる事項について説明を行う。
次に、本発明の第2の実施形態について説明する。なお、以下に記載する第2の実施形態の説明では、上述した第1の実施形態と共通する事項については説明を省略し、上述した第1の実施形態と異なる事項について説明を行う。
第2の実施形態に係る連続体ロボットの概略構成は、図1A及び図1Bに示す第1の実施形態に係る連続体ロボット100の概略構成と同様である。また、第2の実施形態に係る連続体ロボットの制御システムの概略構成は、図3に示す第1の実施形態に係る連続体ロボットの制御システム10の概略構成と同様である。
上述した第1の実施形態では、湾曲可能部170が目標経路610に接近するように後続湾曲区間の目標湾曲角度を設定する形態であった。第2の実施形態では、最遠位湾曲区間の目標湾曲角度に忠実に後続湾曲区間の目標湾曲角度を設定する形態について説明する。
図13は、本発明の第2の実施形態を示し、図3の後続湾曲角度算出部210による最遠位湾曲区間の目標湾曲角度θLの変更に伴う変更後の後続湾曲区間の目標湾曲角度θF'の算出処理の一例を示す図である。なお、以下の図13の説明では、最遠位湾曲区間として図1Aの湾曲区間173を適用し、後続湾曲区間として図1Aの湾曲区間172を適用した場合を想定した例について説明する。また、図13は、図7(a)に示す最遠位湾曲区間の目標湾曲角度θLにおける1度目の変更操作指令がなされた後に2度目の変更操作指令がなされた場合の例を示しているものとする。
図13において、図7(b)と同様に、横軸は、基台部140の変位zbを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の目標湾曲角度θを示している。図13において、破線の「Leader」は、操縦者等により入力装置310を介して入力された最遠位湾曲区間の目標湾曲角度θLに係る湾曲角度のプロファイルを示している。また、図13においては、図7(a)において実線の「Interpolated follower」で示す1回目の変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルを、太い一点鎖線の「First Interpolation」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルとして示している。この図13に示す例は、後続湾曲区間の長さlに満たない基台部140の変位zb(変位aから変位cまでの範囲内の変位)の際に、最遠位湾曲区間の目標湾曲角度θLが最遠位湾曲角度bから最遠位湾曲角度b'に変更された場合を示している。
参照テーブル書換部212は、図13に示す参照テーブルにおいて、最遠位湾曲区間の目標湾曲角度θLが第1の最遠位湾曲角度bから第2の最遠位湾曲角度b'に変更されると、第2の最遠位湾曲角度b'となった際の基台部140の変位zbである変位a'から、後続湾曲区間の長さlの和となる変位である第2変位c'までのうち、第1の最遠位湾曲角度bとなった際の基台部140の変位aから、後続湾曲区間の長さlの和となる変位である第1変位dまでを除いた変位範囲(第1変位dから第2変位c'までの範囲)について、図13に示す太い一点鎖線の「First Interpolation」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルを、第1変位dにおける第1の最遠位湾曲角度bに係る値と第2変位c'における第2の最遠位湾曲角度b'に係る値とを結んだ直線上の目標湾曲角度に係る値で補間する。そして、当該補間後の後続湾曲区間の目標湾曲角度を、実線の「Interpolated follower」で示す変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルとして、図13に示す参照テーブルを書き換える。その後、参照テーブル書換部212は、書き換えた参照テーブルを記憶部211に記憶する。
このように、本実施形態の制御系では、操縦者等による最遠位湾曲区間に対する湾曲操作の全てが補間されて後続湾曲区間へと伝播される。
上述した図13の説明では、最遠位湾曲区間として図1Aの湾曲区間173を適用し、後続湾曲区間として図1Aの湾曲区間172を適用した場合を想定した例について説明した。そして、連続体ロボット100の湾曲可能部170の湾曲区間数が3つ以上、例えば、図1Aに示す湾曲可能部170の湾曲区間数が3つの場合において、上述した後続湾曲区間として図1Aの湾曲区間171を適用する場合には、例えば、上述した最遠位湾曲区間として1つ前の図1Aの湾曲区間172を適用することで実現可能である。
図14は、本発明の第2の実施形態を示し、最遠位湾曲区間の目標湾曲角度θLに係る湾曲角度のプロファイルにおける1度目の変更操作指令と2度目の変更操作指令が同方向の場合の後続湾曲区間の湾曲角度に係る湾曲角度のプロファイルにおけるシミュレーション結果の一例を示す図である。この図14において、横軸は、基台部140の変位zbを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の目標湾曲角度θを示している。また、図14では、破線の「Leader」は、最遠位湾曲区間の目標湾曲角度θLを示し、実線の「Follower」は、最遠位湾曲区間の目標湾曲角度θLの変更に伴う変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルを示している。
図15は、本発明の第2の実施形態を示し、図14に示すシミュレーション結果を反映した連続体ロボット100の湾曲可能部170における動作状態の一例を示す図である。具体的に、図15(a)〜図15(d)は、時系列ごとに連続体ロボット100の湾曲可能部170における動作状態を示したものであり、横軸はx方向の変位を示し、縦軸はz方向の変位を示している。また、図15(a)〜図15(d)において、実線は、連続体ロボット100の湾曲可能部170の湾曲状態を示し、破線は、目標経路610を示している。
図15(a)〜図15(c)の応答は、それぞれ、上述した第1の実施形態における図10(a)〜図10(c)の応答と等しい。図15(d)において、第2の実施形態の制御方法では、操縦者等による湾曲操作を補間しそのまま後続湾曲区間に伝播させているので、第1の実施形態で示したような後続湾曲区間の角速度の急変動は起こらない。そのため、湾曲可能部170の目標経路610の接近は緩やかなものとなる。
図16は、本発明の第2の実施形態を示し、最遠位湾曲区間の目標湾曲角度θLにおける1度目の変更操作指令と2度目の変更操作指令が逆方向の場合の後続湾曲区間の目標湾曲角度におけるシミュレーション結果の一例を示す図である。この図16において、図14で説明した事項と同様の事項については説明を省略する。
図17は、本発明の第2の実施形態を示し、図16に示すシミュレーション結果を反映した連続体ロボット100の湾曲可能部170における動作状態の一例を示す図である。具体的に、図17(a)〜図17(d)は、時系列ごとに連続体ロボット100の湾曲可能部170における動作状態を示したものであり、横軸はx方向の変位を示し、縦軸はz方向の変位を示している。また、図17(a)〜図17(d)において、実線は、連続体ロボット100の湾曲可能部170の湾曲状態を示し、破線は、目標経路610を示している。
図17(a)〜図17(c)の応答は、それぞれ、上述した第1の実施形態における図12(a)〜図12(c)の応答と等しい。図17(d)において、第2の実施形態の制御方法では、第1の実施形態で示したような後続湾曲区間への操作量が低減され伝播されることはない。そのため、湾曲可能部170の目標経路610への接近は、急峻なものとなり、オーバーシュートしうるが、最遠位湾曲区間の操作量そのものに対しては忠実に伝播されている。
第2の実施形態においても、上述した第1の実施形態と同様に、後続湾曲区間の長さに満たない基台部140の変位zbの際に最遠位湾曲区間の目標湾曲角度θLが変更された場合においても、連続体ロボット100の操作性の向上を図ることができる。これにより、連続体ロボット100を目標経路610に沿って動作させる対象物を傷つけたり、また、連続体ロボット100を破損させたりする不具合を回避することが可能となる。
(第3の実施形態)
次に、本発明の第3の実施形態について説明する。なお、以下に記載する第3の実施形態の説明では、上述した第1及び第2の実施形態と共通する事項については説明を省略し、上述した第1及び第2の実施形態と異なる事項について説明を行う。
次に、本発明の第3の実施形態について説明する。なお、以下に記載する第3の実施形態の説明では、上述した第1及び第2の実施形態と共通する事項については説明を省略し、上述した第1及び第2の実施形態と異なる事項について説明を行う。
第3の実施形態に係る連続体ロボットの概略構成は、図1A及び図1Bに示す第1の実施形態に係る連続体ロボット100の概略構成と同様である。また、第3の実施形態に係る連続体ロボットの制御システムの概略構成は、図3に示す第1の実施形態に係る連続体ロボットの制御システム10の概略構成と同様である。
上述した第2の実施形態では、最遠位湾曲区間の目標湾曲角度に忠実に後続湾曲区間の目標湾曲角度を設定する形態であった。第3の実施形態では、湾曲区間の長さlを超えない基台部140の変位zbにおける最遠位湾曲区間に対する湾曲操作を取り消し、後続湾曲区間に伝播させる形態について説明する。
図18は、本発明の第3の実施形態を示し、図3の後続湾曲角度算出部210による最遠位湾曲区間の目標湾曲角度θLの変更に伴う変更後の後続湾曲区間の目標湾曲角度θF'の算出処理の一例を示す図である。なお、以下の図18の説明では、最遠位湾曲区間として図1Aの湾曲区間173を適用し、後続湾曲区間として図1Aの湾曲区間172を適用した場合を想定した例について説明する。また、図18は、図7(a)に示す最遠位湾曲区間の目標湾曲角度θLにおける1度目の変更操作指令がなされた後に2度目の変更操作指令がなされた場合の例を示しているものとする。
図18において、図7(b)と同様に、横軸は、基台部140の変位zbを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の目標湾曲角度θを示している。図18において、破線の「Leader」は、操縦者等により入力装置310を介して入力された最遠位湾曲区間の目標湾曲角度θLに係る湾曲角度のプロファイルを示している。また、図18においては、図7(a)において実線の「Interpolated follower」で示す1回目の変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルを、太い一点鎖線の「First Interpolation」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルとして示している。この図18に示す例は、後続湾曲区間の長さlに満たない基台部140の変位zb(変位aから変位cまでの範囲内の変位)の際に、最遠位湾曲区間の目標湾曲角度θLが最遠位湾曲角度bから最遠位湾曲角度b'に変更された場合を示している。
参照テーブル書換部212は、図18に示す参照テーブルにおいて、最遠位湾曲区間の目標湾曲角度θLが第1の最遠位湾曲角度bから第2の最遠位湾曲角度b'に変更されると、第2の最遠位湾曲角度b'となった際の基台部140の変位zbである第1変位a'から、後続湾曲区間の長さlの和となる変位である第2変位c'までの変位範囲について、図18に示す太い一点鎖線の「First Interpolation」で示す後続湾曲区間の目標湾曲角度θFを、第1変位a'における後続湾曲区間の目標湾曲角度θFに係る値eと第2変位c'における第2の最遠位湾曲角度b'に係る値とを結んだ直線上の目標湾曲角度に係る値で補間する。そして、当該補間後の後続湾曲区間の目標湾曲角度を、実線の「Interpolated follower」で示す変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルとして、図18に示す参照テーブルを書き換える。
図18に示すように、第3の実施形態では、最遠位湾曲区間の目標湾曲角度θLにおける1度目の変更操作指令を後続湾曲区間に伝播するための点dは取り消される。これにより、湾曲区間の長さlを超えない基台部140の変位zbにおいて湾曲操作が連続したとき、最終の湾曲操作時の後続湾曲区間の湾曲角度と最終の湾曲操作による目標湾曲角度が線形補間されて後続湾曲区間に伝播される。
上述した図18の説明では、最遠位湾曲区間として図1Aの湾曲区間173を適用し、後続湾曲区間として図1Aの湾曲区間172を適用した場合を想定した例について説明した。そして、連続体ロボット100の湾曲可能部170の湾曲区間数が3つ以上、例えば、図1Aに示す湾曲可能部170の湾曲区間数が3つの場合において、上述した後続湾曲区間として図1Aの湾曲区間171を適用する場合には、例えば、上述した最遠位湾曲区間として1つ前の図1Aの湾曲区間172を適用することで実現可能である。
図19は、本発明の第3の実施形態を示し、最遠位湾曲区間の目標湾曲角度θLにおける1度目の変更操作指令と2度目の変更操作指令が同方向の場合の後続湾曲区間の目標湾曲角度におけるシミュレーション結果の一例を示す図である。この図19において、横軸は、基台部140の変位zbを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の目標湾曲角度θを示している。また、図19では、破線の「Leader」は、最遠位湾曲区間の目標湾曲角度θLに係る湾曲角度のプロファイルを示し、実線の「Follower」は、最遠位湾曲区間の目標湾曲角度θLの変更に伴う変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルを示している。
図20は、本発明の第3の実施形態を示し、図19に示すシミュレーション結果を反映した連続体ロボット100の湾曲可能部170における動作状態の一例を示す図である。具体的に、図20(a)〜図20(d)は、時系列ごとに連続体ロボット100の湾曲可能部170における動作状態を示したものであり、横軸はx方向の変位を示し、縦軸はz方向の変位を示している。また、図20(a)〜図20(d)において、実線は、連続体ロボット100の湾曲可能部170の湾曲状態を示し、破線は、目標経路610を示している。
図20(a)〜図20(c)の応答は、それぞれ、上述した第1の実施形態における図10(a)〜図10(c)の応答と等しい。図20(d)において、第3の実施形態の制御方法では、最終の湾曲操作時の後続湾曲区間の湾曲角度と、遠位湾曲区間に対する最終の湾曲操作の角度が線形補間されて目標湾曲角度が算出されるため、第1の実施形態で示したような後続湾曲区間の角速度の変動は起こらない。そのため、湾曲可能部170の目標経路610の接近は緩やかなものとなる。
図21は、本発明の第3の実施形態を示し、最遠位湾曲区間の目標湾曲角度θLにおける1度目の変更操作指令と2度目の変更操作指令が逆方向の場合の後続湾曲区間の目標湾曲角度におけるシミュレーション結果の一例を示す図である。この図21において、図19で説明した事項と同様の事項については説明を省略する。
図22は、本発明の第3の実施形態を示し、図21に示すシミュレーション結果を反映した連続体ロボット100の湾曲可能部170における動作状態の一例を示す図である。具体的に、図22(a)〜図22(d)は、時系列ごとに連続体ロボット100の湾曲可能部170における動作状態を示したものであり、横軸はx方向の変位を示し、縦軸はz方向の変位を示している。また、図22(a)〜図22(d)において、実線は、連続体ロボット100の湾曲可能部170の湾曲状態を示し、破線は、目標経路610を示している。
図22(a)〜図22(c)の応答は、それぞれ、上述した第1の実施形態における図12(a)〜図12(c)の応答と等しい。図22(d)において、第3の実施形態の制御方法では、湾曲区間の長さlを超えない基台部140の変位zbにおける修正湾曲操作は伝播されない。そのため、湾曲可能部170の目標経路610への追従性は緩やかなものとなるが、第2の実施形態に示したように後続湾曲区間の湾曲により目標経路610をオーバーシュートすることはない。
第3の実施形態においても、上述した第1の実施形態と同様に、後続湾曲区間の長さに満たない基台部140の変位zbの際に最遠位湾曲区間の目標湾曲角度θLが変更された場合においても、連続体ロボット100の操作性の向上を図ることができる。これにより、連続体ロボット100を目標経路610に沿って動作させる対象物を傷つけたり、また、連続体ロボット100を破損させたりする不具合を回避することが可能となる。
(第4の実施形態)
次に、本発明の第4の実施形態について説明する。なお、以下に記載する第4の実施形態の説明では、上述した第1〜第3の実施形態と共通する事項については説明を省略し、上述した第1〜第3の実施形態と異なる事項について説明を行う。
次に、本発明の第4の実施形態について説明する。なお、以下に記載する第4の実施形態の説明では、上述した第1〜第3の実施形態と共通する事項については説明を省略し、上述した第1〜第3の実施形態と異なる事項について説明を行う。
上述した第1〜第3の実施形態では、xz平面内での連続体ロボット100の先頭追従制御法について説明した。第4の実施形態では、図1A及び図1Bに示すxyzの3次元空間での先頭追従制御を行う。
連続体ロボット100の湾曲角度及び旋回角度を制御するためのアクチュエータによる駆動変位を求めるため、運動学の導出を行う。
本実施形態に用いる記号の定義を以下に示す。
ld:湾曲区間の中心軸の長さ
θn:遠位湾曲区間の湾曲角度
ζn:遠位湾曲区間の旋回角度
ρn:湾曲区間の曲率半径
ζrefn:第nの湾曲区間の遠位端の目標旋回角度
本実施形態に用いる記号の定義を以下に示す。
ld:湾曲区間の中心軸の長さ
θn:遠位湾曲区間の湾曲角度
ζn:遠位湾曲区間の旋回角度
ρn:湾曲区間の曲率半径
ζrefn:第nの湾曲区間の遠位端の目標旋回角度
ここでは、以下の仮定をおき、連続体ロボット100の運動学を導出する。
1.各湾曲区間において、ワイヤは曲率一定に変形する。
2.ワイヤのねじり変形は考慮しない。
3.ワイヤは、長手方向に変形しない。
4.ワイヤガイドとワイヤと間の摩擦は考慮しない。
1.各湾曲区間において、ワイヤは曲率一定に変形する。
2.ワイヤのねじり変形は考慮しない。
3.ワイヤは、長手方向に変形しない。
4.ワイヤガイドとワイヤと間の摩擦は考慮しない。
まず、第1の湾曲区間(図1A及び図1Bの湾曲区間171に相当)のaワイヤ、bワイヤ及びcワイヤの駆動変位lp1a,lp1b及びlp1cと、その遠位端の湾曲角度θ1、並びに、旋回角度ζ1の関係は、以下の(10)式となる。
次に、複数の湾曲区間を有する連続体ロボット100のaワイヤ、bワイヤ及びcワイヤの駆動変位lpna,lpnb及びlpncと、その遠位端の湾曲角度θn、並びに、旋回角度ζnとの関係を求める。湾曲区間数をeとし、第nの湾曲区間を駆動するワイヤの位相角を以下の(11)式とする。
操縦者によって入力装置310を介して最遠位端の第eの湾曲区間に対して、目標湾曲角度θrefe及び目標旋回角度ζrefeが与えられたとき、(12)式を用いてワイヤ駆動変位を求めることで、最遠位端の角度を制御することができる。そして、先頭追従制御は平面駆動の第1〜第3の実施形態と同様に、目標湾曲角度を伝播させればよく、第1〜第3の実施形態の湾曲角度を旋回角度と読み替えて同様のアルゴリズムにより目標角度を算出すればよい。そして、後続湾曲区間の各々に対して、(12)式を用いてワイヤ駆動変位を求めることで、3次元空間内での先頭追従制御を行うことができる。
(その他の実施形態)
その他の実施形態として、記憶部211は、上述した第1〜第3の実施形態に係る先頭追従制御法に用いる参照テーブル2111をそれぞれ記憶しておき、参照テーブル書換部212は、情報入力部213からの入力情報に基づき、使用する実施形態の参照テーブル2111を選択して処理するようにしてもよい。
その他の実施形態として、記憶部211は、上述した第1〜第3の実施形態に係る先頭追従制御法に用いる参照テーブル2111をそれぞれ記憶しておき、参照テーブル書換部212は、情報入力部213からの入力情報に基づき、使用する実施形態の参照テーブル2111を選択して処理するようにしてもよい。
本発明は、上述の実施形態の1以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける1つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、1以上の機能を実現する回路(例えば、ASIC)によっても実現可能である。
このプログラム及び当該プログラムを記憶したコンピュータ読み取り可能な記憶媒体は、本発明に含まれる。
このプログラム及び当該プログラムを記憶したコンピュータ読み取り可能な記憶媒体は、本発明に含まれる。
なお、上述した本発明の実施形態は、いずれも本発明を実施するにあたっての具体化の例を示したものに過ぎず、これらによって本発明の技術的範囲が限定的に解釈されてはならないものである。即ち、本発明はその技術思想、又はその主要な特徴から逸脱することなく、様々な形で実施することができる。
10:連続体ロボットの制御システム、100:連続体ロボット、140:基台部、170:湾曲可能部、171〜173:湾曲区間、200:連続体ロボット制御装置、210:後続湾曲角度算出部、211:記憶部、2111:参照テーブル、212:参照テーブル書換部、213:情報入力部、220:運動学演算部、310〜340:入力装置
Claims (8)
- 基台部と、
遠位用の線状部材が駆動することによって湾曲する遠位湾曲区間と、
前記遠位湾曲区間と前記基台部との間に設けられ、追従用の線状部材が駆動することによって湾曲する追従湾曲区間と、
前記遠位用の線状部材および前記追従用の線状部材を独立に駆動する駆動手段と、
前記基台部、前記遠位湾曲区間、ならびに前記追従湾曲区間を一体にして前進移動させる移動手段と、
制御部と、
を備えた連続体ロボットであって、
前記制御部は、
入力された前記遠位湾曲区間の第1の目標湾曲角度に応じて設定される、前記連続体ロボットの前進移動に対応した前記追従湾曲区間の湾曲角度に関する第1の湾曲角度のプロファイルに従って、前記追従湾曲区間の湾曲角度が前記第1の目標湾曲角度に至るよう構成されており、
前記遠位湾曲区間を前記第1の目標湾曲角度に湾曲した後に、前記第1の湾曲角度のプロファイルに従って前記追従湾曲区間が湾曲および前進移動し、前進移動の変位量が前記追従湾曲区間の長さに相当する第1の変位量に至る前に、前記遠位湾曲区間の目標湾曲角度が、前記第1の目標湾曲角度から第2の目標湾曲角度へ変更された場合、前記第1の湾曲角度のプロファイルとは異なる第2の湾曲角度のプロファイルが設定され、前記連続体ロボットのさらなる前進移動により前記第2の湾曲角度のプロファイルに従って前記追従湾曲区間の湾曲角度が前記第2の目標湾曲角度に至るよう構成されている
ことを特徴とする連続体ロボット。 - 前記制御部は、
前記遠位湾曲区間の目標湾曲角度および前記追従湾曲区間の目標湾曲角度と前記基台部の変位との関係を示す参照テーブルを記憶する記憶手段と、
前記遠位湾曲区間の目標湾曲角度の変更に応じて、前記記憶手段に記憶されている前記参照テーブルを書き換える書換手段と、
を含むことを特徴とする請求項1に記載の連続体ロボット。 - 前記書換手段は、
前記記憶手段に記憶されている前記参照テーブルにおいて、前記遠位湾曲区間の目標湾曲角度が前記第1の目標湾曲角度から前記第2の目標湾曲角度に変更された場合に、前記変更の際の前記基台部の変位である第1変位から、前記第1変位から前記追従湾曲区間の長さだけ進んだ変位である第2変位までを底辺の長さとし、前記第2の目標湾曲角度と前記第1の目標湾曲角度との差を高さとする直角三角形を生成し、
前記直角三角形の斜辺上の点に相当する前記湾曲角度から前記底辺に相当する前記湾曲角度までの前記湾曲角度の変化に係る値を、前記記憶手段に記憶されている前記参照テーブルの前記追従湾曲区間の湾曲角度に係る値に加算し、当該加算後の追従湾曲区間の目標湾曲角度を前記変更後の追従湾曲区間の目標湾曲角度として、前記参照テーブルを書き換えることを特徴とする請求項2に記載の連続体ロボット。 - 前記書換手段は、
前記記憶手段に記憶されている前記参照テーブルにおいて、前記遠位湾曲区間の目標湾曲角度が前記第1の目標湾曲角度から前記第2の目標湾曲角度に変更された場合に、前記第2の目標湾曲角度となった際の前記基台部の変位から、前記追従湾曲区間の長さの和となる変位である第2変位までのうち、前記第1の目標湾曲角度となった際の前記基台部の変位から、前記追従湾曲区間の長さの和となる変位である第1変位までを除いた変位範囲について、前記参照テーブルの前記追従湾曲区間の目標湾曲角度における前記第1変位と前記第2変位との間の変位を、前記第1変位における前記第1の目標湾曲角度に係る値と前記第2変位における前記第2の目標湾曲角度に係る値とを結んだ直線上の前記目標湾曲角度に係る値で補間し、当該補間後の追従湾曲区間の目標湾曲角度を前記変更後の追従湾曲区間の目標湾曲角度として、前記参照テーブルを書き換えることを特徴とする請求項2に記載の連続体ロボット。 - 前記書換手段は、
前記記憶手段に記憶されている前記参照テーブルにおいて、前記遠位湾曲区間の目標湾曲角度が前記第1の目標湾曲角度から前記第2の目標湾曲角度に変更された場合に、前記第2の目標湾曲角度となった際の前記基台部の変位である第1変位から、前記追従湾曲区間の長さの和となる変位である第2変位までの変位範囲について、前記参照テーブルの前記追従湾曲区間の目標湾曲角度における前記第1変位と前記第2変位との間の変位を、前記第1変位における前記追従湾曲区間の目標湾曲角度に係る値と前記第2変位における前記第2の目標湾曲角度に係る値とを結んだ直線上の前記目標湾曲角度に係る値で補間し、当該補間後の追従湾曲区間の目標湾曲角度を前記変更後の追従湾曲区間の目標湾曲角度として、前記参照テーブルを書き換えることを特徴とする請求項2に記載の連続体ロボット。 - 前記記憶手段は、異なる複数の前記参照テーブルを記憶しており、
前記書換手段は、前記複数の参照テーブルの中から1つの参照テーブルを選択し、当該選択した参照テーブルを書き換えることを特徴とする請求項2乃至5のいずれか1項に記載の連続体ロボット。 - 線状部材が駆動することによって湾曲する湾曲区間が複数設けられた湾曲可能部と、前記湾曲可能部を支える基台部と、前記線状部材を駆動する駆動手段と、を備える連続体ロボットの制御方法であって、
複数の前記湾曲区間のうち、前記基台部から最も遠い位置にある遠位湾曲区間の目標湾曲角度と、前記複数の湾曲区間のうち、前記遠位湾曲区間と前記基台部との間に位置する追従湾曲区間の目標湾曲角度と、前記連続体ロボットの進行方向における前記基台部の変位と、前記追従湾曲区間の長さと、に基づいて、前記遠位湾曲区間の目標湾曲角度の変更に伴う変更後の前記追従湾曲区間の目標湾曲角度を算出する算出ステップと、
前記算出ステップで算出された前記変更後の追従湾曲区間の目標湾曲角度に基づいて、前記駆動手段で前記追従湾曲区間の前記線状部材を駆動させる際の駆動変位を演算する演算ステップと、
を有することを特徴とする連続体ロボットの制御方法。 - コンピュータを、請求項1乃至6のいずれか1項に記載の連続体ロボットにおける各手段として機能させるためのプログラム。
Priority Applications (4)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
PCT/JP2021/023313 WO2021261419A1 (ja) | 2020-06-23 | 2021-06-21 | 連続体ロボット及びその制御方法、並びに、プログラム |
EP21829689.5A EP4169679A1 (en) | 2020-06-23 | 2021-06-21 | Continuum robot, control method therefor, and program |
CN202180044714.0A CN115916025A (zh) | 2020-06-23 | 2021-06-21 | 连续体机器人及其控制方法和程序 |
US18/066,134 US20230114672A1 (en) | 2020-06-23 | 2022-12-14 | Continuum robot, control method of the same, and storage medium |
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2020107933 | 2020-06-23 | ||
JP2020107933 | 2020-06-23 |
Publications (1)
Publication Number | Publication Date |
---|---|
JP2022002693A true JP2022002693A (ja) | 2022-01-11 |
Family
ID=79247290
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2021099452A Pending JP2022002693A (ja) | 2020-06-23 | 2021-06-15 | 連続体ロボット及びその制御方法、並びに、プログラム |
Country Status (5)
Country | Link |
---|---|
US (1) | US20230114672A1 (ja) |
EP (1) | EP4169679A1 (ja) |
JP (1) | JP2022002693A (ja) |
CN (1) | CN115916025A (ja) |
WO (1) | WO2021261419A1 (ja) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2023153168A1 (ja) * | 2022-02-14 | 2023-08-17 | キヤノン株式会社 | 連続体ロボット制御システム及び連続体ロボット制御方法 |
Family Cites Families (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6468203B2 (en) | 2000-04-03 | 2002-10-22 | Neoguide Systems, Inc. | Steerable endoscope and improved method of insertion |
WO2010001107A2 (en) * | 2008-06-30 | 2010-01-07 | Oliver Crispin Robotics Limited | Method of controlling a robotic arm |
WO2013158978A1 (en) * | 2012-04-20 | 2013-10-24 | Vanderbilt University | Method and system for compliant insertion of continuum robots |
CN103692440B (zh) * | 2013-12-06 | 2015-06-17 | 中国民航大学 | 一种连续型机器人空间路径跟踪方法 |
US10743750B2 (en) * | 2014-04-28 | 2020-08-18 | Massachusetts Institute Of Technology | Multi-link modular continuum robotic endoscope system |
US11103992B2 (en) * | 2017-02-28 | 2021-08-31 | Canon Kabushiki Kaisha | Apparatus of continuum robot |
US11007641B2 (en) * | 2017-07-17 | 2021-05-18 | Canon U.S.A., Inc. | Continuum robot control methods and apparatus |
-
2021
- 2021-06-15 JP JP2021099452A patent/JP2022002693A/ja active Pending
- 2021-06-21 WO PCT/JP2021/023313 patent/WO2021261419A1/ja unknown
- 2021-06-21 EP EP21829689.5A patent/EP4169679A1/en active Pending
- 2021-06-21 CN CN202180044714.0A patent/CN115916025A/zh active Pending
-
2022
- 2022-12-14 US US18/066,134 patent/US20230114672A1/en active Pending
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2023153168A1 (ja) * | 2022-02-14 | 2023-08-17 | キヤノン株式会社 | 連続体ロボット制御システム及び連続体ロボット制御方法 |
Also Published As
Publication number | Publication date |
---|---|
WO2021261419A1 (ja) | 2021-12-30 |
EP4169679A1 (en) | 2023-04-26 |
US20230114672A1 (en) | 2023-04-13 |
CN115916025A (zh) | 2023-04-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP7155323B2 (ja) | 制御装置及び制御方法 | |
JP6921602B2 (ja) | 連続体ロボットの制御システム及びその制御方法、並びに、プログラム | |
JP6596126B2 (ja) | 連続体ロボット制御方法および装置 | |
JP2018008335A (ja) | 連続体ロボット、その運動学の補正方法、および連続体ロボットの制御方法 | |
JP6733046B2 (ja) | 連続体ロボット、連続体ロボットの運動学モデルの補正方法、および連続体ロボットの制御方法 | |
WO2019221036A1 (ja) | 連続体ロボットおよび連続体ロボットの制御システム | |
WO2021261419A1 (ja) | 連続体ロボット及びその制御方法、並びに、プログラム | |
JP2022008179A (ja) | 連続体ロボットの制御システム及びその制御方法、並びに、プログラム | |
US20230131269A1 (en) | Continuum robot control system and control method, and program | |
WO2023112620A1 (ja) | 連続体ロボット制御システム、連続体ロボット制御方法及びプログラム | |
WO2023153168A1 (ja) | 連続体ロボット制御システム及び連続体ロボット制御方法 | |
WO2023022056A1 (ja) | 連続体ロボットの制御システム及びその制御方法 | |
WO2023164047A1 (en) | Method and apparatus for controlling continuum robot | |
JP2023130055A (ja) | 連続体ロボット及びその制御方法、並びに、プログラム | |
JP6398777B2 (ja) | ロボット制御装置、制御方法、及び制御プログラム |