JP7379208B2 - 連続体ロボットの制御システム及びその制御方法、並びに、連続体ロボット - Google Patents
連続体ロボットの制御システム及びその制御方法、並びに、連続体ロボット Download PDFInfo
- Publication number
- JP7379208B2 JP7379208B2 JP2020028813A JP2020028813A JP7379208B2 JP 7379208 B2 JP7379208 B2 JP 7379208B2 JP 2020028813 A JP2020028813 A JP 2020028813A JP 2020028813 A JP2020028813 A JP 2020028813A JP 7379208 B2 JP7379208 B2 JP 7379208B2
- Authority
- JP
- Japan
- Prior art keywords
- control system
- wire
- continuum robot
- continuum
- gain
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims description 28
- 230000007246 mechanism Effects 0.000 claims description 102
- 238000006073 displacement reaction Methods 0.000 claims description 62
- 238000005452 bending Methods 0.000 claims description 28
- 238000012546 transfer Methods 0.000 claims description 28
- 238000004364 calculation method Methods 0.000 claims description 10
- 238000010586 diagram Methods 0.000 description 57
- 230000004044 response Effects 0.000 description 38
- 238000006243 chemical reaction Methods 0.000 description 29
- 238000013461 design Methods 0.000 description 28
- 230000006870 function Effects 0.000 description 26
- 238000013016 damping Methods 0.000 description 15
- 238000004088 simulation Methods 0.000 description 11
- 238000001514 detection method Methods 0.000 description 8
- 238000003780 insertion Methods 0.000 description 5
- 230000037431 insertion Effects 0.000 description 5
- 230000008901 benefit Effects 0.000 description 4
- 230000007423 decrease Effects 0.000 description 4
- 238000009795 derivation Methods 0.000 description 4
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 238000005381 potential energy Methods 0.000 description 2
- 230000008859 change Effects 0.000 description 1
- 230000008602 contraction Effects 0.000 description 1
- 239000012636 effector Substances 0.000 description 1
- 239000013641 positive control Substances 0.000 description 1
- 230000036544 posture Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 210000002435 tendon Anatomy 0.000 description 1
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/1628—Programme controls characterised by the control loop
- B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
-
- 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
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J17/00—Joints
- B25J17/02—Wrist joints
- B25J17/0241—One-dimensional joints
- B25J17/025—One-dimensional joints mounted in series
-
- 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/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
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Health & Medical Sciences (AREA)
- Life Sciences & Earth Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Surgery (AREA)
- Optics & Photonics (AREA)
- Biomedical Technology (AREA)
- Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
- Physics & Mathematics (AREA)
- Pathology (AREA)
- Radiology & Medical Imaging (AREA)
- Orthopedic Medicine & Surgery (AREA)
- Biophysics (AREA)
- Heart & Thoracic Surgery (AREA)
- Medical Informatics (AREA)
- Molecular Biology (AREA)
- Animal Behavior & Ethology (AREA)
- Public Health (AREA)
- Veterinary Medicine (AREA)
- Manipulator (AREA)
- Feedback Control In General (AREA)
Description
本発明は、連続体ロボットの制御システム及びその制御方法、並びに、連続体ロボットに関するものである。
連続体ロボットは、コンティニュウムロボットとも呼ばれ、柔軟性のある構造を持つ湾曲可能部を有し、その湾曲可能部を変形させることにより形状が制御される。この連続体ロボットは、剛体リンクによって構成されるロボット(以下、「剛体リンクロボット」と記載する)に対して、主に2つの優位性を持つ。1つ目の優位性は、連続体ロボットは、剛体リンクロボットがはまり込んでしまうような狭い空間や散乱物のある環境の中で、その先端の操作のみで対象物の曲線に沿って移動可能である点である。2つ目の優位性は、連続体ロボットは、本質的な柔らかさを有するため、特に開空間においては脆弱な対象物に損傷を与えることなく操作することができる点である。そこでは、剛体リンクロボットで必要とされるエンドエフェクタでの外力検出は、必ずしも必要とされない。
この特徴を生かし、連続体ロボットは、内視鏡のシースやカテーテルなどの医療の分野や、レスキューロボットなどの極限作業ロボットへの応用が期待されている。この連続体ロボットの駆動方法としては、腱駆動によるもの、押し引き可能なワイヤによるもの、空気アクチュエータによるものなどがある。
特許文献1には、駆動用のワイヤに発生する負荷量を検出し、負荷量が予め設定された範囲となるように、駆動部を制御するマニピュレータの制御方法が記載されている。これにより、特許文献1では、マニピュレータを狭小空間に挿通する際に、その経路に沿うための湾曲操作をすることなく挿入が可能とし、挿通動作を容易にしている。ここで、駆動部が負荷量に応じて受動的に動作することを「バックドライバビリティがある」というが、特許文献1では、クラッチなどの機構ではなく、ワイヤに発生する負荷量の検出と制御アルゴリズムを用いており、駆動部の大型化を防いでいる。具体的に、特許文献1に記載のマニピュレータは、筒状部とよばれる連続体と、その先端の複数の関節構造からなり、関節に連結されるワイヤを基端部筐体に設けられたモータにより牽引することで動作し、また、複数の節輪と回動関節で連結される連続体に対して適用可能としている。
しかしながら、特許文献1に記載のマニピュレータでは、上述した挿通動作を行う「挿通動作モード」と、操作者が湾曲操作ボタンを用いて先端湾曲部の操作を行う「動作制御モード」との切り替えが必要であり、また、駆動部がバックドライバビリティを持つのは「挿通動作モード」時のみである。そして、特許文献1に記載のマニピュレータにおいて、「挿通動作モード」時には、湾曲操作ボタンが無効化され、先端湾曲部の操作を行うことはできない。これにより、特許文献1に記載のマニピュレータでは、分岐がある狭小な経路の選択をするときや、緩やかな経路と急峻な経路とが繰り返されるときには、上述した切り替え操作が煩雑となるとともに、例えば大湾曲動作時にバックドライバビリティが低く湾曲可能部の目標位置への位置決め性能が低いと、誤った操作により脆弱な対象物やマニピュレータ自体が損傷するという問題がある。
本発明は、このような問題点に鑑みてなされたものであり、操作者が煩雑な操作を行わずに、湾曲可能部の目標位置への高い位置決め性能を実現できる仕組みを提供することを目的とする。
本発明の連続体ロボットの制御システムは、ワイヤで駆動され、湾曲可能に構成された少なくとも1つの湾曲可能部と、前記ワイヤを駆動する駆動部とを備える連続体ロボットの制御システムであって、前記駆動部による前記ワイヤの押し引き駆動の目標変位と、前記連続体ロボットから得られる前記ワイヤを把持するワイヤ把持機構の変位との誤差を補償する制御を行うものであって、前記ワイヤの目標張力を出力する位置制御部と、前記位置制御部から出力される前記ワイヤの目標張力と、前記連続体ロボットから得られる前記ワイヤの張力との誤差を補償する制御を行う力制御部と、を有し、前記力制御部を含む第1のループ制御系と、前記力制御部および前記位置制御部を含む第2のループ制御系とが構成されている。
また、本発明は、上述した連続体ロボット及び上述した連続体ロボットの制御システムによる連続体ロボットの制御方法を含む。
また、本発明は、上述した連続体ロボット及び上述した連続体ロボットの制御システムによる連続体ロボットの制御方法を含む。
本発明によれば、操作者が煩雑な操作を行わずに、湾曲可能部の目標位置への高い位置決め性能を実現することができる。
以下に、図面を参照しながら、本発明を実施するための形態(実施形態)について説明する。
(第1の実施形態)
まず、本発明の第1の実施形態について説明する。
まず、本発明の第1の実施形態について説明する。
図1は、本発明の第1の実施形態に係る連続体ロボットの制御システム10の概略構成の一例を示す図である。以下、図1に示す連続体ロボットの制御システム10を「連続体ロボットの制御システム10-1」として記載する。
連続体ロボットの制御システム10-1は、図1に示すように、連続体ロボットの機構部(以下、単に「連続体ロボット」と記載することもある)100、連続体ロボットの制御装置200、及び、入力装置300を有して構成されている。また、連続体ロボットの制御装置200は、図1に示すように、運動学演算部210、位置制御部KSV、力制御部KFを有して構成されている。
本実施形態では、図1に示す力制御部KFを含むインナーループ制御系(第1のループ制御系)で連続体ロボット100に対する力制御を行う。また、図1に示す力制御部KF及び位置制御部KSVを含むアウターループ制御系(第2のループ制御系)で連続体ロボット100に対する位置制御を行う。本実施形態に係る連続体ロボットの制御システム10-1では、このインナーループ制御系(第1のループ制御系)及びアウターループ制御系(第2のループ制御系)の二重ループ制御系により、連続体ロボット100に対して、バックドライバビリティが高く、同時に、連続体ロボット100における湾曲可能部の目標位置への高い位置決め性能を実現する。
第1の実施形態では、ワイヤで駆動され、湾曲可能に構成された湾曲可能部と、ワイヤを駆動する駆動部とを備えた連続体ロボット100の動的モデルPnを導出する。この際、駆動部は、ワイヤの張力の検出が可能であり、例えば、回転モータと回転直動変換機構を含み構成されうる。
運動学演算部210は、入力装置300から、連続体ロボット100の湾曲可能部(後述する図2の湾曲可能部110)における目標湾曲角度refθが入力されると、運動学演算を行い、連続体ロボット100の駆動部によるワイヤの押し引き駆動の目標変位refzを出力する。位置制御部KSVは、運動学演算部210から出力された目標変位refzと、連続体ロボット100の変位センサから得られるワイヤを把持するワイヤ把持機構の変位zt2との誤差を補償する制御を行うものであって、ワイヤの目標張力に対応する目標発生力refFを出力する。ここで、目標発生力refFは、本実施形態の中で定義するワイヤ張力センサの正負反転値である発生力Fの目標値を表す。力制御部KFは、位置制御部KSVから出力された目標発生力refFと、連続体ロボット100から得られるワイヤの張力に対応する発生力Fとの誤差を補償する制御を行うものであって、駆動部の目標トルクであるモータ目標トルクTmを出力する。そして、連続体ロボット100の回転モータ等に制御入力を与える。
上述したように、力制御部KFを含むインナーループ制御系(第1のループ制御系)では、目標発生力refFと連続体ロボット100のワイヤ張力センサから得られる発生力Fとの差分を取ることで誤差を演算し、力制御部KFは、その誤差を補償する制御入力としてモータ目標トルクTmを出力する。このフィードバックループ制御系は、回転直動変換機構の等価慣性を補償することと等価であり、これにより連続体ロボット100のバックドライバビリティを向上することを可能とする。また、本実施形態では、このインナーループ制御系(第1のループ制御系)のフィードバックによる閉ループ制御系をGclと表す。また、この閉ループ制御系Gclと位置制御部KSVを含むアウターループ制御系(第2のループ制御系)では、目標変位refzと連続体ロボット100の変位センサから得られる変位zt2との差分を取ることで誤差を演算し、位置制御部KSVは、その誤差を補償する制御入力として目標発生力refFを出力する。ここで、力制御部KF及び位置制御部KSVの設計には、連続体ロボット100の動的モデルPnから得られる伝達関数を用いることでフィードバック系の安定性を保証する。これにより、連続体ロボット100における湾曲可能部の外乱に対する高いバックドライビリティを有しつつ、目標位置への高い位置決め性能を実現することができる。
以下に、連続体ロボット100の動的モデルPnの導出と制御アルゴリズムについて詳細に記述する。
1.1)モデリング
図2は、本発明の第1の実施形態に係る連続体ロボット100の概略構成の一例を示す図である。以下、図2に示す連続体ロボット100を「連続体ロボット100-1」として記載する。
図2は、本発明の第1の実施形態に係る連続体ロボット100の概略構成の一例を示す図である。以下、図2に示す連続体ロボット100を「連続体ロボット100-1」として記載する。
連続体ロボット100-1は、湾曲可能部110の遠位端160における固定部121及び固定部122に、それぞれ、ワイヤ111及びワイヤ112が接続されている。また、ワイヤ111及びワイヤ112の近位端は、ロボット基台部140の中で、それぞれ、ワイヤ把持パイプ131及びワイヤ把持パイプ132に接続されている。ロボット基台部140には、ワイヤ把持パイプ131を支持するワイヤ把持機構171、ワイヤ把持パイプ132を支持するワイヤ把持機構172、及び、駆動部に相当するアクチュエータ180が配置されている。この際、ワイヤ把持機構172は、ロボット基台部140に固定されている。また、ワイヤ把持機構171は、駆動部であるアクチュエータ180にワイヤ把持機構基台(不図示)を介して接続され、上下に移動可能である。ワイヤ把持パイプ131は、ワイヤ把持機構171に支持可能に接続され、アクチュエータ180で押引きすることにより湾曲可能部110の姿勢が制御される。また、連続体ロボット100-1は、ワイヤ111及びワイヤ112を案内するための部材であるワイヤガイド161~164を有して構成されている。このワイヤガイドは、複数の部材を離散的に配置する方法の他に、蛇腹状やメッシュ状などの連続体部材を用いてもよい。ワイヤガイド161~164は、ワイヤ112に、固定部150~153において固定される。さらに、ワイヤ111及びワイヤ112の間隔と、ワイヤ把持機構171及びワイヤ把持機構172との間隔が異なってもよい。そのときは、径変換部190をロボット基台部140に接続する。
本実施形態では、ワイヤ111及びワイヤ112と、ワイヤガイド161~164からなる機構を連続体部分である湾曲可能部110とよぶ。また、アクチュエータ180には、回転モータと回転直動変換機構からなるアクチュエータユニットを用いるものとする。さらに、ワイヤ把持機構171(或いはアクチュエータ180)は、ワイヤ111の張力を検出する機能を持つものとする。そのためには、ワイヤ把持機構171とアクチュエータ180との間に、上述したワイヤ把持機構基台(不図示)を設け、このワイヤ把持機構基台をアクチュエータ180に接続し、ワイヤ把持機構基台とワイヤ把持機構171との間をばねで接続する。このとき、ワイヤ把持機構171がz軸方向にのみ変位するように、リニアガイドを設けることや、ばねに平行ばねを用いることが好ましい。そして、ばねの変位を計測することでワイヤ111の張力が検出可能となる。
図3は、本発明の第1の実施形態における連続体ロボット100の動的モデルPnの一例を示す図である。具体的に、図3(a)は、連続体部分である湾曲可能部110の動的モデルの一例を示し、また、図3(b)は、アクチュエータ180に含まれる回転モータ及び回転直動変換機構とワイヤ把持機構171等からなる動的モデルの一例を示す。
1.1.1)連続体部分である湾曲可能部110の動的モデル
図3(a)に示す連続体部分である湾曲可能部110の動的モデルにおける符号の定義を以下に記載する。
θ:連続体ロボットの湾曲可能部110の湾曲角度
ρ:曲率半径
l:湾曲可能部110の長さ
r1:湾曲可能部110の中心軸からワイヤまでの距離
lp:ワイヤの駆動量
ms:湾曲可能部110の質量
kb:湾曲可能部110の湾曲角度に対するばね係数
zw:ワイヤ等価質量の変位
zp:ワイヤ把持パイプの変位
mw:ワイヤの質量
mp:ワイヤ把持パイプの質量
kw,kw3:ワイヤのばね係数
cw,cw3:ワイヤの減衰係数
cw2:ワイヤと径変換部及びワイヤガイドとの摩擦による減衰係数
図3(a)に示す連続体部分である湾曲可能部110の動的モデルにおける符号の定義を以下に記載する。
θ:連続体ロボットの湾曲可能部110の湾曲角度
ρ:曲率半径
l:湾曲可能部110の長さ
r1:湾曲可能部110の中心軸からワイヤまでの距離
lp:ワイヤの駆動量
ms:湾曲可能部110の質量
kb:湾曲可能部110の湾曲角度に対するばね係数
zw:ワイヤ等価質量の変位
zp:ワイヤ把持パイプの変位
mw:ワイヤの質量
mp:ワイヤ把持パイプの質量
kw,kw3:ワイヤのばね係数
cw,cw3:ワイヤの減衰係数
cw2:ワイヤと径変換部及びワイヤガイドとの摩擦による減衰係数
続いて、連続体ロボット100-1の運動方程式を導出する。本実施形態では、以下の仮定をおく。
[1]2次元平面内での運動のみを考慮する。
[2]湾曲可能部110の曲率は一定とし、ばね係数は均一であるものとする。
[3]ワイヤは集中質点系として近似し、長手方向の変形による反力が湾曲可能部110の先端に作用するものとする。ワイヤの横振動及び横方向の変形は考慮しない。
[4]ワイヤとワイヤガイドとの間、及び、ワイヤと径変換部との間の摩擦は、クーロン摩擦等の非線形性摩擦を含めて粘性減衰として減衰係数cw2に押し込める。
[1]2次元平面内での運動のみを考慮する。
[2]湾曲可能部110の曲率は一定とし、ばね係数は均一であるものとする。
[3]ワイヤは集中質点系として近似し、長手方向の変形による反力が湾曲可能部110の先端に作用するものとする。ワイヤの横振動及び横方向の変形は考慮しない。
[4]ワイヤとワイヤガイドとの間、及び、ワイヤと径変換部との間の摩擦は、クーロン摩擦等の非線形性摩擦を含めて粘性減衰として減衰係数cw2に押し込める。
次に、ワイヤ及びワイヤ把持パイプの運動エネルギーTwと位置エネルギーUwは、ワイヤ駆動量がlp=r1θであるから、それぞれ、以下の(8)式及び(9)式となる。
そして、運動方程式をラグランジュの方程式を示す以下の(10)式
より求めると、以下の(11)式に示す非線形微分方程式が得られる。以下の(11)式において、Φは、Mc,Cc,Kcに入れることのできない非線形項とする。
また、(11)式のMc22及びΦ3は、以下の(12)式及び(13)式となる。
しかしながら、湾曲可能部110の湾曲角度が0度の近傍では、(12)式及び(13)式は不定形となり、値を求めることができない。そこで、大変形を考慮しない0度近傍線形化モデルを求める。(4)式及び(5)式より、以下の(14)式となる。
そして、湾曲可能部110の0度近傍での運動エネルギーTa0は、以下の(15)式となる。
そして、T=Ta0+Twとおいて、ラグランジュの方程式より運動方程式を求めると、以下の(16)式となる。
1.1.2)回転モータ及び回転直動変換機構とワイヤ把持機構からなる動的モデル
図3(b)は、回転モータ及び回転直動変換機構とワイヤ把持機構からなる動的モデルの一例を示す図である。ワイヤ把持機構は、ワイヤの張力を検出する機能を持ち、ワイヤ把持機構とアクチュエータ180との間にワイヤ把持機構基台を設け、ワイヤ把持機構基台をアクチュエータ180に接続し、ワイヤ把持機構基台とワイヤ把持機構との間をばねで接続する。そのばねの変位を検出することにより張力検出を行う。図3(b)に示す動的モデルにおける符号の定義を以下に記載する。
Jm:モータのイナーシャ
θm:モータ回転角度
Tm:モータ目標トルク(指令値)
cm:モータ軸の減衰係数
kg,cg:カップリングのばね係数,減衰係数
Jn:駆動軸のイナーシャ
θn:駆動軸の回転角度
cn:駆動軸の減衰係数
p:駆動軸のねじピッチ
R:直動回転変換機構の変換係数であって等価的増速比
mt1:ワイヤ把持機構基台の質量
kt1:駆動軸のz方向のばね係数
ct1:ワイヤ把持機構基台のリニアスライダの減衰係数
zt1:ワイヤ把持機構基台の変位
mt2:ワイヤ把持機構の質量
kt2,ct2:張力検出機構のばね係数
zt2:ワイヤ把持機構の変位
図3(b)は、回転モータ及び回転直動変換機構とワイヤ把持機構からなる動的モデルの一例を示す図である。ワイヤ把持機構は、ワイヤの張力を検出する機能を持ち、ワイヤ把持機構とアクチュエータ180との間にワイヤ把持機構基台を設け、ワイヤ把持機構基台をアクチュエータ180に接続し、ワイヤ把持機構基台とワイヤ把持機構との間をばねで接続する。そのばねの変位を検出することにより張力検出を行う。図3(b)に示す動的モデルにおける符号の定義を以下に記載する。
Jm:モータのイナーシャ
θm:モータ回転角度
Tm:モータ目標トルク(指令値)
cm:モータ軸の減衰係数
kg,cg:カップリングのばね係数,減衰係数
Jn:駆動軸のイナーシャ
θn:駆動軸の回転角度
cn:駆動軸の減衰係数
p:駆動軸のねじピッチ
R:直動回転変換機構の変換係数であって等価的増速比
mt1:ワイヤ把持機構基台の質量
kt1:駆動軸のz方向のばね係数
ct1:ワイヤ把持機構基台のリニアスライダの減衰係数
zt1:ワイヤ把持機構基台の変位
mt2:ワイヤ把持機構の質量
kt2,ct2:張力検出機構のばね係数
zt2:ワイヤ把持機構の変位
1.1.3)拡大系の構成
図3(c)に示すように、湾曲可能部110の動的モデルと、回転モータ及び回転直動変換機構とワイヤ把持機構からなる動的モデルとを結合する。ワイヤ把持機構の変位zt2とワイヤ把持パイプの変位zpを同一とすることで、(11)式と(21)式によって拡大系を構成すると、運動方程式は、以下の(22)式のようになる。ここで、qg=[ql T,zw,θ]Tとしている。
図3(c)に示すように、湾曲可能部110の動的モデルと、回転モータ及び回転直動変換機構とワイヤ把持機構からなる動的モデルとを結合する。ワイヤ把持機構の変位zt2とワイヤ把持パイプの変位zpを同一とすることで、(11)式と(21)式によって拡大系を構成すると、運動方程式は、以下の(22)式のようになる。ここで、qg=[ql T,zw,θ]Tとしている。
次に、状態方程式を求める。湾曲可能部110の湾曲角度が0度の近傍にないときは、非線形項Φgを行列Kgに押し込めて線形化を行う、以下の(23)式の拡張線形化法を適用する。
これにより、Φ3は、Kg(6,6)の要素となる。そして、状態方程式は、以下の(24)式及び(25)式となる。
本実施形態では、ワイヤ把持機構基台の変位zt1及びワイヤ把持機構の変位zt2が測定可能とし、ばねkt2の発生する力がワイヤ把持機構に及ぼす力(以下、発生力)F、及びワイヤ把持機構の変位zt2を観測量とする。ここで、発生力Fは、以下の(26)式となる。
湾曲可能部110の湾曲角度が0度の近傍にあるときは、湾曲可能部110の0度近傍での運動方程式を示す(16)式と、回転モータ及び回転直動変換機構とワイヤ把持機構からなる動的モデルの運動方程式を示す(21)式により、同様に拡大系を構成する。これは線形モデルであるので、そのまま状態方程式に変換すればよい。
1.2)モデル解析
1.2.1)等価的増速比Rに対する変動
本章では、モデル解析を行い、回転直動変換機構による等価的増速比Rによる連続体部分である湾曲可能部110の極と零点の変動について検討する。本実施形態では、さらに、ワイヤと径変換部及びワイヤガイドとの摩擦係数cw2による位相の変動について説明する。ここで、湾曲可能部110の最低次の固有振動数は62Hzであものとする。このとき、回転直動変換機構の等価的増速比Rを設計パラメータの1倍から10000倍まで変化させることで回転直動変換機構の等価慣性を変化させ、極と零点の変動を調べる。また、この解析では、極と零点の変動を分かりやすくするために、湾曲可能部110の各部の減衰係数をほぼ0としている。
1.2.1)等価的増速比Rに対する変動
本章では、モデル解析を行い、回転直動変換機構による等価的増速比Rによる連続体部分である湾曲可能部110の極と零点の変動について検討する。本実施形態では、さらに、ワイヤと径変換部及びワイヤガイドとの摩擦係数cw2による位相の変動について説明する。ここで、湾曲可能部110の最低次の固有振動数は62Hzであものとする。このとき、回転直動変換機構の等価的増速比Rを設計パラメータの1倍から10000倍まで変化させることで回転直動変換機構の等価慣性を変化させ、極と零点の変動を調べる。また、この解析では、極と零点の変動を分かりやすくするために、湾曲可能部110の各部の減衰係数をほぼ0としている。
図4は、本発明の第1の実施形態における連続体ロボット100の動的モデルPnの極-零点応答(極-零点配置)の一例を示す図である。この図4において、湾曲可能部110単体の極を星印で示し、等価的増速比Rを10000倍とする極と零点をそれぞれアスタリスクと丸印で示し、等価的増速比Rを1倍とする極と零点をそれぞれX印と三角印で示す。これより、モータ側の等価慣性が大きくなるにつれて湾曲可能部110の固有振動数は低下し、過減衰の極と、原点極に移動することがわかる。ここで、丸印及び三角印で示す零点は、ほぼ移動していないことに注目されたい。湾曲可能部110の振動数は過減衰となるが、この零点による制御帯域内での位相特性を制御系設計に考慮する必要がある。
図5は、本発明の第1の実施形態における連続体ロボット100の動的モデルPnを示すボード線図である。具体的には、図5は、等価的増速比Rを変動させる拡大系のボード線図を示す。ここで、拡大系は、(24)式、(27)式を、湾曲角度θを0.05度として線形化している。図5では、制御入力であるモータ目標トルクTmから発生力Fまでの伝達関数を示し、等価的増速比Rを10倍、50倍、500倍とする応答を、それぞれ、実線、破線、一点鎖線で示している。等価的増速比Rが小さくなり、モータ側の等価慣性が大きくなるにつれて、連続体のピークが減少していき、また、反共振周波数は変わらないことがわかる。実線で示す等価的増速比Rを10倍する応答では、約18Hzにある反共振の要因がわかりづらいが、破線で示す等価的増速比Rを500倍とする応答では、反共振は湾曲可能部110と張力検出機構を結合した拡大系における連続体部分の共振・反共振特性によるものであることがわかる。そして、この反共振周波数において位相が180度進むことがわかる。
1.2.2)ワイヤと径変換部及びワイヤガイドとの摩擦による減衰係数cw2に対する変動
次に、減衰係数cw2によるモデルの変動について検討する。
図6は、本発明の第1の実施形態における連続体ロボット100の動的モデルPnを示すボード線図である。具体的には、図6は、減衰係数cw2を変動させる拡大系のボード線図を示す。図6では、制御入力であるモータ目標トルクTmから発生力Fまでの伝達関数を示し、減衰係数cw2を0.01,1,10,100とする応答を、それぞれ、実線、点線、破線、一点鎖線で示している。減衰係数cw2が大きくなるに従って、低周波数域が積分特性から1次遅れ特性へと変化する。これに伴い、約18Hzの零点周波数付近を境に位相が180度反転してゆく。これが目標とするサーボ帯域付近であれば、制御系の設計に大きく影響する。回転直動変換機構を有する連続体ロボット100-1の制御系設計においては、湾曲可能部110の固有振動数に加え、回転直動変換機構及び径変換部のモデル同定が重要となることがわかる。
次に、減衰係数cw2によるモデルの変動について検討する。
図6は、本発明の第1の実施形態における連続体ロボット100の動的モデルPnを示すボード線図である。具体的には、図6は、減衰係数cw2を変動させる拡大系のボード線図を示す。図6では、制御入力であるモータ目標トルクTmから発生力Fまでの伝達関数を示し、減衰係数cw2を0.01,1,10,100とする応答を、それぞれ、実線、点線、破線、一点鎖線で示している。減衰係数cw2が大きくなるに従って、低周波数域が積分特性から1次遅れ特性へと変化する。これに伴い、約18Hzの零点周波数付近を境に位相が180度反転してゆく。これが目標とするサーボ帯域付近であれば、制御系の設計に大きく影響する。回転直動変換機構を有する連続体ロボット100-1の制御系設計においては、湾曲可能部110の固有振動数に加え、回転直動変換機構及び径変換部のモデル同定が重要となることがわかる。
図7は、本発明の第1の実施形態における連続体ロボット100の動的モデルPnを示すボード線図である。具体的には、図7は、拡大系のボード線図を示す。モータ目標トルクTmから発生力Fまでの伝達関数を実線で示し、ワイヤ把持機構の変位zt2までを破線で示し、湾曲可能部110の先端角度θまでを一点鎖線で示している。高周波数域には、ワイヤ張力検出機構のばね、ワイヤ、回転直動変換機構による高次振動が現れている。
1.3)制御系設計
本章では、図1に示すように、力制御部KFを含むインナーループ制御系(第1のループ制御系)で力制御を行い、力制御部KF及び位置制御部KSVを含むアウターループ制御系(第2のループ制御系)で位置制御を行う、二重ループ制御系によって、連続体ロボット100-1に対して、バックドライバビリティが高く、同時に、連続体ロボット100における湾曲可能部の目標位置への高い位置決め性能を実現する制御系の設計を行う。ここで、動的モデルPnは、(24)式,(27)式に示した拡大系を表す。
本章では、図1に示すように、力制御部KFを含むインナーループ制御系(第1のループ制御系)で力制御を行い、力制御部KF及び位置制御部KSVを含むアウターループ制御系(第2のループ制御系)で位置制御を行う、二重ループ制御系によって、連続体ロボット100-1に対して、バックドライバビリティが高く、同時に、連続体ロボット100における湾曲可能部の目標位置への高い位置決め性能を実現する制御系の設計を行う。ここで、動的モデルPnは、(24)式,(27)式に示した拡大系を表す。
ここで、拡大系の動的モデルPnと力制御部KFを含むインナーループ制御系(第1のループ制御系)では、発生力Fの目標値である目標発生力refFと発生力Fとの差分を取ることで誤差を演算し、力制御部KFは、その誤差を補償する制御入力としてモータ目標トルクTmを出力する。このフィードバックループは、回転直動変換機構の等価慣性を補償することと等価であり、これにより、連続体ロボット100-1のバックドライバビリティを向上することを可能とする。また、本実施形態では、上述したように、このフィードバックによる閉ループ系をGclと表す。また、この閉ループ制御系Gclと位置制御部KSVを含むアウターループ制御系(第2のループ制御系)では、目標変位refzと連続体ロボット100から得られる変位zt2との差分を取ることで誤差を演算し、位置制御部KSVは、その誤差を補償する制御入力として目標発生力refFを出力する。
本実施形態では、力制御を行うインナーループ制御系(第1のループ制御系)には、例えば、以下の(30)式に示すPI制御系を用いる。
ここで、Fziは、積分制御器の零交差周波数である。また、前章で示した高次モードに対する安定化のために折点周波数200Hzの2次のローパスフィルタをPI制御系に結合する。回転直動変換機構を用いることによる湾曲可能部110の極、零点の変動と、その零点付近で減衰係数cw2により変動する位相特性に対して、安定な力制御部KFを設計するためには、開ループ伝達関数PnKFを導出し、その応答からゲイン余裕、位相余裕が十分であるように設計する(安定余裕の演算をする)。
図8は、本発明の第1の実施形態における連続体ロボットの制御装置200を示すボード線図である。具体的に、図8(a)及び図8(b)は、(30)式のKp,Fziを、それぞれ、2.2‐10-3,1.0とし、制御帯域を約30Hzとする力制御部KFのボード線図、及び、拡大系Pnとの開ループ伝達関数PnKFのボード線図を示す。
また、本実施形態では、位置制御を行うアウターループ制御系(第2のループ制御系)には、例えば、以下の(31)式に示すPID制御系を用いる。
ここで、Fzi及びFzdは、それぞれ、積分制御器及び微分制御器の零交差周波数である。拡大系の動的モデルPnと力制御部KFとの閉ループ伝達関数Gclを用いて、開ループ伝達関数GclKSVを導出し、その応答からゲイン余裕、位相余裕が十分であるように設計する。また、折点角周波数50Hzの1次のローパスフィルタをPID制御系に結合する。
図9は、本発明の第1の実施形態における連続体ロボットの制御装置200を示すボード線図である。具体的に、図9(a)及び図9(b)は、(31)式のKp,Fzi,Fzdを、それぞれ、5‐103,0.5,100とし、制御帯域を約3.5Hzとする位置制御部KSVのボード線図、及び、閉ループ系Gclとの開ループ伝達関数GclKSVのボード線図を示す。
また、図10は、本発明の第1の実施形態における連続体ロボットの制御装置200に係る制御系の設計手順を示すフローチャートである。この図10には、以下の設計手順が記載されている。
<1>拡大系の動的モデルPnの導出
<2>係数Kp,Kiを設定し、ローパスフィルタを結合し、力制御部KFを仮設計
<3>開ループ伝達関数PnKFを導出
<4>ゲイン余裕、位相余裕は十分かの判断
<5>力制御部KFの設計完了
<6>閉ループ系Gclを導出
<7>係数Kp,Ki,Kdを設定し、ローパスフィルタを結合し、位置制御部KSVを仮設計
<8>開ループ伝達関数GclKSVを導出
<9>ゲイン余裕、位相余裕は十分かの判断
<10>位置制御部KSVの設計完了
<1>拡大系の動的モデルPnの導出
<2>係数Kp,Kiを設定し、ローパスフィルタを結合し、力制御部KFを仮設計
<3>開ループ伝達関数PnKFを導出
<4>ゲイン余裕、位相余裕は十分かの判断
<5>力制御部KFの設計完了
<6>閉ループ系Gclを導出
<7>係数Kp,Ki,Kdを設定し、ローパスフィルタを結合し、位置制御部KSVを仮設計
<8>開ループ伝達関数GclKSVを導出
<9>ゲイン余裕、位相余裕は十分かの判断
<10>位置制御部KSVの設計完了
1.4)シミュレーション
1.3)制御系設計で設計した制御系と、(24)式,(27)式に示した拡大系を用いてシミュレーションを行った。
1.3)制御系設計で設計した制御系と、(24)式,(27)式に示した拡大系を用いてシミュレーションを行った。
図11は、本発明の第1の実施形態を示し、力制御部KFを用いるインナーループ制御系(第1のループ制御系)のみの外乱応答を示す図である。具体的に、図11(a)にモータ回転角度θmを示し、図11(b)にワイヤ把持機構の変位zt2を示し、図11(c)に発生力Fを示し、図11(d)に制御入力Tmを示し、図11(e)に湾曲可能部110の先端の湾曲角度θを示し、図11(f)に湾曲可能部110の先端に与える外乱トルクを示す。また、図11では、制御応答を実線で、比較として非制御時の応答を破線で示し、目標発生力refFを点線で示す。
シミュレーションの開始後、5秒で図11(f)に示すように湾曲可能部110の先端部に外乱トルクを与える。このとき、図11に示すように、ワイヤ把持機構には、張力検出機構のばねによりz軸負方向の発生力が作用するが、発生力Fの目標値は0である。そのため、図11(d)に示すように力制御部KFは、正の制御入力をモータに与え、図11(a)に示すようにモータが正方向に回転することで、図11(b)に示すようにワイヤ把持機構は、z軸正方向に変位する。これにより、図11(c)に示すように発生力Fが目標値である0に補償されることで、図11(e)に示すように、湾曲可能部110の先端は、外乱トルクと同方向に湾曲し、外乱トルクに応じてモータがバックドライブする制御が実現している。破線で示す非制御時の応答では、図11(f)の外乱トルクを同様に与えているが、回転直動変換機構の等価慣性が大きいため、図11(a),図11(e)に破線で示すように、モータは、ほとんど回転せず、湾曲可能部110の先端はほとんど湾曲しない。これより、力制御を行うインナーループ制御系は、回転直動変換機構の等価慣性を補償し、連続体先端の外乱トルクに対するバックドライブを可能としていることがわかる。
次に、図12は、本発明の第1の実施形態を示し、力制御部KFを用いるインナーループ制御系(第1のループ制御系)と更に位置制御部KSVを含むアウターループ制御系(第2のループ制御系)との二重ループ制御系による外乱を伴う位置決め応答を示す図である。具体的に、図12(a)~図12(f)に、図11(a)~図11(f)と同様の配置で各応答を示す。このシミュレーションでは、モータ角度θmのみ位置制御を行うセミクローズド制御との比較を示す。二重ループ制御系とセミクローズド制御系の位置制御系のサーボ帯域は、同様になるように設計している。図12では、二重ループ制御系の制御応答を実線で示し、セミクローズド制御系の応答を破線で示す。また、図12(a)にセミクローズド制御系に与えるモータの目標角度refmを点線で示し、図12(b)にアウターループに与えるワイヤ把持機構の目標変位refzを点線で示し、図12(c)に制御系KSVが出力する制御入力である発生力Fの目標値である目標発生力refFを点線で示す。
シミュレーションの開始後、二重ループ制御系とセミクローズド制御系は、ともに目標軌道に追従し、約1秒で目標変位に整定する。その後、1.5秒で図12(f)に示すように湾曲可能部110の端部に外乱トルクを与える。このとき、図12(c)に示すように、アウターループ制御系(第2のループ制御系)の位置制御部KSVは、ワイヤ把持機構の変位zt2に発生した変位誤差を補償するために、z軸負方向の発生力目標値を制御入力として出力する。図12(c)に示すように、インナーループ制御系(第1のループ制御系)の力制御部KFは、発生力Fの目標値である目標発生力refFに追従する。しかしながら、同時に回転直動変換機構の等価慣性を補償し低減しているため、図12(a)に示すように、モータは、正方向にバックドライブし、図12(b)に示すように、ワイヤ把持機構は、z軸正方向に繰り出される。図12(d)の1.5秒での正方向のモータへの制御入力は、等価慣性を低減する制御が行われていることを表している。これにより、図12(e)に示すように、湾曲可能部110の先端は、外乱トルクと同方向に湾曲し、外乱トルクに対してバックドライブする制御系が実現していることがわかる。破線で示すセミクローズド制御系は、図12(f)の外乱トルクが加わると、回転直動変換機構の等価慣性が大きいため、図12(a),図12(e)に破線で示すように、モータは、ほとんど回転せず、湾曲可能部110の先端もほとんど湾曲しない。図12(f)の発生力Fが二重ループ制御系とほぼ等しいのは、位置決め制御系のサーボ帯域がほぼ等しいであるためである。これより、二重ループ制御系は、モータ角度θmのみ位置制御を行うセミクローズド制御系と同様の目標位置への整定性能を持ち、同時に、湾曲可能部110の先端の外乱トルクに対するバックドライブを可能としていることがわかる。
以上説明したように、第1の実施形態に係る連続体ロボットの制御システム10では、力制御部KFを含むインナーループ制御系(第1のループ制御系)と、力制御部KF及び位置制御部KSVを含むアウターループ制御系(第2のループ制御系)とを構成するようにしている。
かかる構成によれば、操作者が煩雑な操作を行わずに、湾曲可能部の目標位置への高い位置決め性能を実現することができる。その結果、連続体ロボット100の湾曲可能部先端の外乱に対する高いバックドライビリティを有することができる。
かかる構成によれば、操作者が煩雑な操作を行わずに、湾曲可能部の目標位置への高い位置決め性能を実現することができる。その結果、連続体ロボット100の湾曲可能部先端の外乱に対する高いバックドライビリティを有することができる。
(第2の実施形態)
次に、本発明の第2の実施形態について説明する。なお、以下に記載する第2の実施形態の説明では、上述した第1の実施形態と共通する事項については説明を省略し、上述した第1の実施形態と異なる事項について説明を行う。
次に、本発明の第2の実施形態について説明する。なお、以下に記載する第2の実施形態の説明では、上述した第1の実施形態と共通する事項については説明を省略し、上述した第1の実施形態と異なる事項について説明を行う。
上述した第1の実施形態では、連続体部分である湾曲可能部110の振動特性が回転直動変換機構と連成することにより、低周波数域に移動し、さらにワイヤと径変換部及びワイヤガイドとの摩擦により位相特性が変動することを示した。また、上述した第1の実施形態では、力制御を行うインナーループ制御系(第1のループ制御系)の力制御部KFにPI制御系を用いた。
これに対して、第2の実施形態は、力制御部KFにPID制御系を用いて位相特性を補償することを考える形態である。ここでは、例えば、以下の(32)式に示すPID制御系を用いる。
ここで、Fzi及びFzdは、それぞれ、積分制御器及び微分制御器の零交差周波数である。また、折点角周波数200Hzの2次のローパスフィルタをPID制御系(PID制御器)に結合(例えば直列結合)する。第1の実施形態と同様に、開ループ伝達関数PnKFを導出し、その応答からゲイン余裕、位相余裕が十分であるように設計するが、第2の実施形態では、ゲインKdを設計する方法を示す。PID制御系において、微分制御器は、零交差周波数Fzdよりも低い周波数ではゲイン(微分ゲイン)をほとんど上げずに、位相を進めることができる。しかしながら、Fzdよりも高い周波数では、ゲインを持ち上げてしまう。そのため、零交差周波数Fzdを変化させ、その都度開ループ伝達関数PnKFを用いてゲイン余裕を演算する(安定余裕の演算をする)ことで零交差周波数Fzdとゲイン余裕の関係を取得し、ゲイン余裕を最大とする零交差周波数Fzdを探索する。
図13は、本発明の第2の実施形態における連続体ロボットの制御装置200に係る制御系のパラメータ探索を示す図である。具体的に、図13は、最小周波数を第1の実施形態で示した連続体部分である湾曲可能部110の零点よりも十分低い1Hzとし、最大周波数を例えば制御系をデジタル実装する際のナイキスト周波数である1kHzとするゲイン余裕の探索の応答を示す。図13では、零交差周波数Fzdの95Hz付近で、ゲイン余裕が最大となることがわかる。
図14は、本発明の第2の実施形態における連続体ロボットの制御装置200に係る制御系のボード線図及び開ループ応答を示す図である。具体的に、図14(a)に力制御部KFのPID制御系のボード線図を示し、図14(b)に開ループ伝達関数PnKのボード線図を示す。図14(a)及び図14(b)において、実線で零交差周波数Fzdを95Hzとする第2の実施形態のPID制御系の場合を示し、破線で第1の実施形態のPI制御系の場合を示している。開ループ応答より、微分特性で位相遅れを補償することで、ゲイン余裕が約7dB改善していることがわかる。
また、図15は、本発明の第2の実施形態における連続体ロボットの制御装置200に係る制御系の設計手順を示すフローチャートである。この図15には、以下の設計手順が記載されている。
<1>拡大系の動的モデルPnの導出
<2>係数Kp,Kiを設定し、ローパスフィルタを結合し、力制御部KFを仮設計
<3>開ループ伝達関数PnKFを導出
<4>ゲイン余裕、位相余裕は十分かの判断
<5>零交差周波数Fzdの探索範囲を決定
<6>零交差周波数Fzdを探索し、ゲイン余裕を最大とするKdを決定
<7>力制御部KFの設計完了
<8>閉ループ系Gclを導出
<9>係数Kp,Ki,Kdを設定し、ローパスフィルタを結合し、位置制御部KSVを仮設計
<10>開ループ伝達関数GclKSVを導出
<11>ゲイン余裕、位相余裕は十分かの判断
<12>位置制御部KSVの設計完了
<1>拡大系の動的モデルPnの導出
<2>係数Kp,Kiを設定し、ローパスフィルタを結合し、力制御部KFを仮設計
<3>開ループ伝達関数PnKFを導出
<4>ゲイン余裕、位相余裕は十分かの判断
<5>零交差周波数Fzdの探索範囲を決定
<6>零交差周波数Fzdを探索し、ゲイン余裕を最大とするKdを決定
<7>力制御部KFの設計完了
<8>閉ループ系Gclを導出
<9>係数Kp,Ki,Kdを設定し、ローパスフィルタを結合し、位置制御部KSVを仮設計
<10>開ループ伝達関数GclKSVを導出
<11>ゲイン余裕、位相余裕は十分かの判断
<12>位置制御部KSVの設計完了
(第3の実施形態)
次に、本発明の第3の実施形態について説明する。なお、以下に記載する第3の実施形態の説明では、上述した第1及び第2の実施形態と共通する事項については説明を省略し、上述した第1及び第2の実施形態と異なる事項について説明を行う。
次に、本発明の第3の実施形態について説明する。なお、以下に記載する第3の実施形態の説明では、上述した第1及び第2の実施形態と共通する事項については説明を省略し、上述した第1及び第2の実施形態と異なる事項について説明を行う。
上述した第2の実施形態では、PID制御系の微分要素により位相遅れ特性を補償した。第3の実施形態では、制御系の次数を増やし、更に位相遅れを補償する。また、改善されたゲイン余裕に相当するように、力制御を行うインナーループ制御系(第1のループ制御系)のゲインを上げ、インナーループ制御系の制御帯域を拡大する。そして、シミュレーションにより有効性を検証する。
3.1)制御系設計
上述した第2の実施形態で設計した力制御部KFに、以下の(33)式で示す位相進みフィルタを結合(例えば、直列結合)する。本実施形態では、PID-Lead制御系と表記する。
ここで、Fc1は零交差周波数であり、Fc2(>Fc1)は、フィルタをプロパーとするための極である。本実施形態では、Fc2を1kHzとし、Fc1を探索する。第2の実施形態と同様に、零交差周波数Fc1を変化させ、その都度ゲイン余裕を演算することで零交差周波数Fc1とゲイン余裕との関係を取得し、ゲイン余裕を最大とする零交差周波数Fc1を探索する。
上述した第2の実施形態で設計した力制御部KFに、以下の(33)式で示す位相進みフィルタを結合(例えば、直列結合)する。本実施形態では、PID-Lead制御系と表記する。
図16は、本発明の第3の実施形態における連続体ロボットの制御装置200に係る制御系のパラメータ探索を示す図である。具体的に、図16は、最小周波数を連続体部分である湾曲可能部110の零点よりも十分低い1Hzとし、最大周波数Fc2以下となる900Hzとする探索応答を示す。図16では、零交差周波数Fc1の540Hz付近で、ゲイン余裕が最大となることがわかる。応答が急峻であるので、若干の余裕をみて、零交差周波数Fc1を545Hzとすると、ゲイン余裕は約6dB改善する。
図17は、本発明の第3の実施形態における連続体ロボットの制御装置200に係る制御系のボード線図及び開ループ応答を示す図である。具体的に、図17(a)に力制御部KFのPID-Lead制御系のボード線図を示し、図17(b)に開ループ伝達関数PnKのボード線図を示す。図17(a)及び図17(b)において、実線でPID-Lead制御系を用いる力制御系を示し、破線で第1の実施形態の制御系を示している。ここで、PID-Lead制御系は、第1の実施形態のPI制御系から改善されたゲイン余裕に相当する13dBのゲインを上げている。そのため、第1の実施形態の制御系に比べハイゲインとなるが、ゲイン余裕は等しい。
また、図18は、本発明の第3の実施形態における連続体ロボットの制御装置200に係る制御系の設計手順を示すフローチャートである。この図18には、以下の設計手順が記載されている。
<1>拡大系の動的モデルPnの導出
<2>係数Kp,Kiを設定し、ローパスフィルタを結合し、力制御部KFを仮設計
<3>開ループ伝達関数PnKFを導出
<4>ゲイン余裕、位相余裕は十分かの判断
<5>ゲイン余裕GM1を記憶
<6>零交差周波数Fzdの探索範囲を決定
<7>零交差周波数Fzdを探索し、ゲイン余裕を最大とするKdを決定
<8>位相進みフィルタの周波数Fc1の探索範囲と、周波数Fc2を決定
<9>Fc1を探索し、ゲイン余裕を最大位相進みフィルタを決定
<10>ゲイン余裕GM2を記憶
<11>仮設計の力制御部KFにGM2-GM1相当のゲインを乗じる
<12>力制御部KFの設計完了
<13>閉ループ系Gclを導出
<14>係数Kp,Ki,Kdを設定し、ローパスフィルタを結合し、位置制御部KSVを仮設計
<15>開ループ伝達関数GclKSVを導出
<16>ゲイン余裕、位相余裕は十分かの判断
<17>位置制御部KSVの設計完了
<1>拡大系の動的モデルPnの導出
<2>係数Kp,Kiを設定し、ローパスフィルタを結合し、力制御部KFを仮設計
<3>開ループ伝達関数PnKFを導出
<4>ゲイン余裕、位相余裕は十分かの判断
<5>ゲイン余裕GM1を記憶
<6>零交差周波数Fzdの探索範囲を決定
<7>零交差周波数Fzdを探索し、ゲイン余裕を最大とするKdを決定
<8>位相進みフィルタの周波数Fc1の探索範囲と、周波数Fc2を決定
<9>Fc1を探索し、ゲイン余裕を最大位相進みフィルタを決定
<10>ゲイン余裕GM2を記憶
<11>仮設計の力制御部KFにGM2-GM1相当のゲインを乗じる
<12>力制御部KFの設計完了
<13>閉ループ系Gclを導出
<14>係数Kp,Ki,Kdを設定し、ローパスフィルタを結合し、位置制御部KSVを仮設計
<15>開ループ伝達関数GclKSVを導出
<16>ゲイン余裕、位相余裕は十分かの判断
<17>位置制御部KSVの設計完了
3.2)シミュレーション
図19は、本発明の第3の実施形態を示し、力制御部KFを用いるインナーループ制御系(第1のループ制御系)と更に位置制御部KSVを含むアウターループ制御系(第2のループ制御系)との二重ループ制御系による外乱を伴う位置決め応答を示す図である。具体的に、図19(a)~図19(f)に、図11(a)~図11(f)と同様の配置で各応答を示す。このシミュレーションでは、第1の実施形態のPI制御系との比較を示す。PID-Lead制御系を用いる二重ループ制御系のアウターループ制御系(第2のループ制御系)のサーボ帯域は、第1の実施形態と同様になるように設計している。図19では、PI-Lead制御系の応答を実線で示し、PI制御系の応答を破線で示す。また、図19(b)にアウターループに与えるワイヤ把持機構の目標変位refzを点線で示し、図19(c)に位置制御部KSVが出力する制御入力である目標発生力refFを点線で示す。
図19は、本発明の第3の実施形態を示し、力制御部KFを用いるインナーループ制御系(第1のループ制御系)と更に位置制御部KSVを含むアウターループ制御系(第2のループ制御系)との二重ループ制御系による外乱を伴う位置決め応答を示す図である。具体的に、図19(a)~図19(f)に、図11(a)~図11(f)と同様の配置で各応答を示す。このシミュレーションでは、第1の実施形態のPI制御系との比較を示す。PID-Lead制御系を用いる二重ループ制御系のアウターループ制御系(第2のループ制御系)のサーボ帯域は、第1の実施形態と同様になるように設計している。図19では、PI-Lead制御系の応答を実線で示し、PI制御系の応答を破線で示す。また、図19(b)にアウターループに与えるワイヤ把持機構の目標変位refzを点線で示し、図19(c)に位置制御部KSVが出力する制御入力である目標発生力refFを点線で示す。
シミュレーションの開始後、PI-Lead制御系とPI制御系は、ともに目標軌道に追従し、約1秒で目標変位に整定する。その後、1.5秒で図19(f)に示すように、湾曲可能部110の先端部に外乱トルクを与える。第1の実施形態と同様に、図19(c)に示すように、アウターループ制御系(第2のループ制御系)の位置制御部KSVは、z軸負方向の発生力Fの目標値である目標発生力refFを制御入力として出力し、インナーループ制御系(第1のループ制御系)の力制御部KFは、発生力Fの目標値に追従する。そして、力制御部KFは、同時に回転直動変換系の等価慣性を補償し低減しているため、図19(a)に示すように、モータは、正方向にバックドライブし、図19(b)に示すように、ワイヤ把持機構は、z軸正方向に繰り出される。ここで、PID-Lead制御系は、PI制御系に比べてハイゲインであるため大きくバックドライブしていることがわかる。図19(d)のモータへの制御入力より、PID-Lead制御系は、PI制御系に比べて等価慣性を低減する制御入力が大きいことがわかる。これにより、図19(e)に示すように、PID-Lead制御系は、湾曲可能部110の先端への外乱トルクに対して大きなバックドライバビリティをもつ制御を実現している。また、PID-Lead制御系は、高次の振動モードに対する安定性も損なわれていない。
そして、PID-Lead制御系は、位相遅れ特性を高次で補償することにより、インナーループ制御系(第1のループ制御系)の力制御部KFをハイゲインに設計できることから、回転直動変換機構の等価慣性をハイゲインに補償し、バックドライバビリティを増加させることが可能である。
(第4の実施形態)
次に、本発明の第4の実施形態について説明する。なお、以下に記載する第4の実施形態の説明では、上述した第1~第3の実施形態と共通する事項については説明を省略し、上述した第1~第3の実施形態と異なる事項について説明を行う。
次に、本発明の第4の実施形態について説明する。なお、以下に記載する第4の実施形態の説明では、上述した第1~第3の実施形態と共通する事項については説明を省略し、上述した第1~第3の実施形態と異なる事項について説明を行う。
上述した第1及び第2の実施形態では、二重ループ制御系により、連続体ロボット100にバックドライバビリティを与えることができることを示した。さらに、第3の実施形態では、インナーループ制御系(第1のループ制御系)の力制御部KFをハイゲインに設計することにより、バックドライバビリティを増加させることが可能であることを示した。本実施形態では、位置決め性能を変えることなく、バックドライバビリティの大きさを調整可能とする制御系を示す。
まず、第1~第3の実施形態に示したように、二重ループ制御系を設計する。そして、力制御部KFに0以上1以下の係数(第1の係数)αを乗ずる。これにより、力制御部KFのゲインが下がり、バックドライバビリティの大きさを任意に減少できる。しかしながら、同時に開ループ伝達関数GclKSVのゲインが下がるため、位置決め制御のサーボ帯域が下がってしまう。そこで、位置制御部KSVのゲインを上げ、係数αが1であるときと同様のサーボ帯域となるように再設計する。これにより、位置制御の性能は変えずに、バックドライバビリティを変動させることが可能である。
図20は、本発明の第4の実施形態に係る連続体ロボットの制御システム10の概略構成の一例を示す図である。以下、図20(a)に示す連続体ロボットの制御システム10を「連続体ロボットの制御システム10-2」として記載する。また、図20(b)に示す連続体ロボットの制御システム10を「連続体ロボットの制御システム10-3」として記載する。図20(c)に示す連続体ロボットの制御システム10を「連続体ロボットの制御システム10-4」として記載する。この図20(a)~図20(c)において、図1に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。
図20(a)に示す連続体ロボットの制御システム10-2において、破線は、力制御部KFの変動に応じて位置制御部KSVを再設計すること示している。具体的に、図20(a)では、力制御部KFのゲインに上述した係数(第1の係数)αを乗じで第1のゲインとする様子が示されている。この際、図20(a)では、係数(第1の係数)αは、入力装置400から力制御部KFに入力されるようになっている。そして、計算機上では、位置制御部KSVのゲイン(第2のゲイン)を細かなステップで増加させ、開ループ伝達関数GclKSVの応答を算出することにより、係数αが1であるときと同様のサーボ帯域となること確認する、という手順を繰り返して反復すればよい。ただし、係数αがあまりに小さいと、位置制御部KSVがハイゲインにならざるを得ず、高次モードを励起し制御系が不安定になってしまう。そのため、あらかじめ係数αの下限を設定しておけばよい。もしくは、係数αに応じて位置制御部KSVのサーボ帯域が常に等しくなるように、位置制御部KSVのゲインに乗ずる係数(第2の係数)を予め算出して、メモリに保存しておく。この場合、例えば、図20(b)に示す連続体ロボットの制御システム10-3のように、係数αに応じてメモリ220から、位置制御部KSVのゲインに乗ずる係数(第2の係数)を読みだせばよい。これにより、操作者が係数αを任意に指令できるつまみなどを連続体ロボット100の操作系に付加することで、実時間にバックドライビリティを変更できる。
次に、湾曲可能部110の湾曲角度に連動してバックドライバビリティを変動する制御系を示す。例えば、大きく湾曲するにつれてバックドライバビリティを低くし、外乱等に対する変形を防ぐには、以下の(34)式とすることが考えられる。
また、逆に、大きく湾曲するにつれてバックドライバビリティを高くし、安全性を高めるためるには、以下の(35)式とすることが考えられる。
ここで、β,γは係数であり、予め湾曲の範囲を設定し、係数αが0<α<1の範囲に収まるように設定する必要がある。図20(c)に、そのブロック線図を示す。ここで、係数演算部fαは、(34)式,(35)式に示すように、湾曲可能部110における目標湾曲角度refθに応じて係数(第1の係数)αを演算して変化させる手段を表している。
図21は、本発明の第4の実施形態に係る連続体ロボットの制御システム10に係る制御系によるシミュレーション応答を示す図である。第1の実施形態と同様に、位置決め終了後の1.5秒において、例えば図12(f)に示す外乱トルクを湾曲可能部110の先端に与えている。図21では、湾曲可能部110の先端部分の湾曲角度を示し、係数αを1、0.6、0.3、0.1とする応答を、それぞれ、点線、実線、破線、一点鎖線で示している。係数αの変化に伴う位置決め性能の変化はないが、係数αが小さくなるにつれて、バックドライバビリティが低くなっていることがわかる。これより、位置決め性能を変えることなく、バックドライバビリティの大きさを調整可能とする制御系が実現できることがわかる。
(第5の実施形態)
次に、本発明の第5の実施形態について説明する。なお、以下に記載する第5の実施形態の説明では、上述した第1~第4の実施形態と共通する事項については説明を省略し、上述した第1~第4の実施形態と異なる事項について説明を行う。
次に、本発明の第5の実施形態について説明する。なお、以下に記載する第5の実施形態の説明では、上述した第1~第4の実施形態と共通する事項については説明を省略し、上述した第1~第4の実施形態と異なる事項について説明を行う。
上述した第1~第4の実施形態では、ワイヤ把持機構基台及び先端側の変位zt1,zt2を観測量としていた。しかしながら、機構の制約上それを実現できる変位センサを配置できないことがある。そこで、本実施形態では、他の観測量を用いる制御系について説明する。
ワイヤ把持機構の変位zt2が観測困難であり、張力検出機構のばね変位kt2がひずみゲージなどにより観測可能であれば、その発生力Fを観測量とすればよい。そして、この場合、ワイヤ把持機構の変位zt2は、以下の(36)式により求められる。
また、ワイヤ把持機構の移動範囲に比べて力検出部のばね変位が実用上十分に小さいときは、アウターループ制御系(第2のループ制御系)の制御量をワイヤ把持機構基台の変位zt1としてもよい。
さらに、ワイヤ把持機構基台の変位zt1の観測が困難であり、カップリングのばね係数及び駆動軸のz方向のばね係数kg,kt1が十分に大きい場合には、モータの回転角度θmを観測し、以下の(37)式と近似し、アウターループ制御系(第2のループ制御系)の位置制御量として用いてもよい。
(第6の実施形態)
次に、本発明の第6の実施形態について説明する。なお、以下に記載する第6の実施形態の説明では、上述した第1~第5の実施形態と共通する事項については説明を省略し、上述した第1~第5の実施形態と異なる事項について説明を行う。
次に、本発明の第6の実施形態について説明する。なお、以下に記載する第6の実施形態の説明では、上述した第1~第5の実施形態と共通する事項については説明を省略し、上述した第1~第5の実施形態と異なる事項について説明を行う。
上述した第1~第5の実施形態では、平面内駆動の連続体ロボット100に対して制御系設計を行った。本実施形態では、立体駆動可能な連続体ロボットに対して制御系設計を行う。
図22は、本発明の第6の実施形態に係る連続体ロボット100の概略構成の第1例を示す図である。以下、図22に示す連続体ロボット100を「連続体ロボット100-2」として記載する。
図22に示す連続体ロボット100-2は、湾曲可能部1100の遠位端1060における固定部1021~1023にワイヤ1011~1013が接続されている。また、ワイヤ1011~1013の近位端は、ワイヤ把持パイプ1031~1033が接続されている。ロボット基台部1040には、第1の実施形態と同様に、ワイヤ1011~1013における各ワイヤごとに、ワイヤ把持機構(図22では不図示)及びアクチュエータ(図22では不図示)が設置されている。ここで、ワイヤ把持機構は、アクチュエータにワイヤ把持機構基台(図22では不図示)を介して接続され、上下に移動可能である。ワイヤ把持パイプ1031~1033は、ワイヤ把持機構に接続され、アクチュエータで押引きすることにより姿勢が制御される。また、連続体ロボット100-2は、ワイヤ1011~1013を案内するための部材であるワイヤガイド1061~1064を有する。このワイヤガイドは、複数の部材を離散的に配置する方法の他に、蛇腹状やメッシュ状などの連続体部材を用いてもよい。ワイヤガイド1061~1064は、ワイヤ1011に、固定部1050~1053において固定される。さらに、ワイヤ1011~1013の間隔と、ワイヤ把持パイプ1031~1033との間隔が異なってもよい。そのときには、径変換部1190をロボット基台部1040に接続すればよい。また、図22において、連続体ロボット100-2の中心軸を破線で示す。
本実施形態では、ワイヤ1011~1013と、ワイヤガイド1061~1064からなる機構を連続体部分である湾曲可能部1100とよぶ。また、不図示のアクチュエータには、回転モータと回転直動変換機構からなるアクチュエータユニットを用いるものとする。さらに、ワイヤ把持機構は、ワイヤの張力を検出する機能を持つものとする。そのためには、ワイヤ把持機構とアクチュエータとの間に上述したワイヤ把持機構基台を設け、ワイヤ把持機構基台をアクチュエータに接続し、ワイヤ把持機構基台とワイヤ把持機構の間をばねで接続する。このとき、ワイヤ把持機構がz軸方向にのみ変位するように、リニアガイドを設けることや、ばねに平行ばねを用いることが好ましい。そして、ばねの変位を計測することでワイヤの張力が検出可能となる。以下に、図22に示す符号の定義を記載する。
ld:湾曲可能部1100の中心軸の長さ
θn:湾曲可能部1100の遠位端の湾曲角度
ζn:湾曲可能部1100遠位端の旋回角度
ρn:湾曲可能部1100の曲率半径
ld:湾曲可能部1100の中心軸の長さ
θn:湾曲可能部1100の遠位端の湾曲角度
ζn:湾曲可能部1100遠位端の旋回角度
ρn:湾曲可能部1100の曲率半径
また、本実施形態では、ワイヤ1011~1013をxy面内において反時計回りにaワイヤ、bワイヤ、cワイヤと呼び、そのa~cワイヤ1011~1013の駆動変位をlp1a,lp1b,lp1cとする。
図23は、本発明の第6の実施形態に係る連続体ロボット100のワイヤの配置の一例を示す図である。この図23に示すように、a~cワイヤ1011~1013は、一片の長さをrsとする正三角形の頂点に配置されており、位相角ξnは、ワイヤの配置を決定する角度である。本実施形態では、ξ1=0とする。
そして、本実施形態では、以下の仮定をおき、連続体ロボット100-2の運動学を導出する。
[1]各湾曲可能部110において、ワイヤは曲率一定に変形する。
[2]ワイヤのねじり変形は考慮しない。
[3]ワイヤは長手方向に変形しない。
[4]ワイヤガイドとワイヤとの間の摩擦は考慮しない。
[1]各湾曲可能部110において、ワイヤは曲率一定に変形する。
[2]ワイヤのねじり変形は考慮しない。
[3]ワイヤは長手方向に変形しない。
[4]ワイヤガイドとワイヤとの間の摩擦は考慮しない。
本実施形態では、a~cワイヤ1011~1013における各ワイヤに独立して、第1~第5の実施形態の二重ループ制御を適用する分散制御を行う。
図24は、本発明の第6の実施形態に係る図22に示す連続体ロボット100-2を含む連続体ロボットの制御システム10の概略構成の一例を示す図である。以下、図24に示す連続体ロボットの制御システム10を「連続体ロボットの制御システム10-5」として記載する。この図24において、図1に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。
この図24において、湾曲可能部110の遠位端の目標湾曲角度refθ1及び目標旋回角度refζ1が入力装置510から運動学演算部210に入力されている。また、各ワイヤ把持機構の目標変位refza,refzb,refzcは、(38)式の湾曲角度θ1、旋回角度ζ1に、それぞれ、目標湾曲角度refθ1、目標旋回角度refζ1を代入することで得られる。動的モデルPnaは、bワイヤ,cワイヤのワイヤ把持パイプをアクチュエータ基台部に固定し、aワイヤを駆動するものとして第1の実施形態の動的モデルPnと等価として得られるモデルである。同様に、動的モデルPnbは、aワイヤ,cワイヤのワイヤ把持パイプをアクチュエータ基台部に固定し、bワイヤを駆動するものとして第1の実施形態の動的モデルPnと等価として得られるモデルである。同様に、動的モデルPncは、aワイヤ,bワイヤのワイヤ把持パイプをアクチュエータ基台部に固定し、cワイヤを駆動するものとして第1の実施形態の動的モデルPnと等価として得られるモデルである。また、KFa,KFb,KFcは、それそれ、aワイヤ,bワイヤ,cワイヤに対する力制御部である。この力制御部は、個別に設計してもよいし、特に、動的モデルPna,Pnb,Pncに差がなければ、同じ制御系でもよい。また、KSVa,KSVb,KSVcは、それそれ、aワイヤ,bワイヤ,cワイヤに対する位置制御部である。この位置制御部は、個別に設計してもよいし、特に、動的モデルPna,Pnb,Pncに差がなければ、同一の制御系でもよい。
また、係数αa,αb,αcは、第4の実施形態で示した、バックドライバビリティを可変とするための係数である。具体的に、図24では、係数αaは、入力装置520から力制御部KFa等に入力され、係数αbは、入力装置530から力制御部KFb等に入力され、係数αcは、入力装置540から力制御部KFc等に入力されるようになっている。この際、係数αa,αb,αcのそれぞれを異なる値に設定すれば、旋回方向に応じてバックドライバビリティを可変にすることが可能となる。また、係数αa,αb,αcのそれぞれを全て同じ値にすれば、旋回方向に関わりなく、均一なバックドライバビリティを得ることができる。
ここで、3本のワイヤのうち、1本のワイヤのバックドライバビリティを大きく下げれば、連続体ロボット100-2の中心軸方向の外乱に対するバックドライバビリティのみを下げ、湾曲方向の外乱トルクにはバックドライバビリティを持たせることができる。これは、湾曲可能部1100が基台部に衝突することを回避したり、逆に、湾曲可能部1100がz軸正方向に繰り出されてワイヤ把持機構が基台部に衝突したりすることを回避することができる。
さらに、連続体ロボット100-2の中心軸方向の外乱に対するバックドライバビリティを0とし、湾曲方向の外乱にはバックドライバビリティを持たせるには、1本のワイヤもしくはワイヤ把持パイプをアクチュエータ基台部に固定すればよい。
図25は、本発明の第6の実施形態に係る連続体ロボット100の概略構成の第2例を示す図である。以下、図25に示す連続体ロボット100を「連続体ロボット100-3」として記載する。この図25において、図22に示す構成と同様の構成については同じ符号を付している。具体的に、図25は、aワイヤ1011を固定部1191に固定する例を示す。このとき、湾曲可能部1100遠位端の湾曲角度θ1及び旋回角度ζ1とするための、bワイヤの駆動変位lp1b,cワイヤの駆動変位lp1cは、それぞれ、以下の(39)式及び(40)式となる。
図26は、本発明の第6の実施形態に係る図25に示す連続体ロボット100-3を含む連続体ロボットの制御システム10の概略構成の一例を示す図である。図26では、各ワイヤ把持機構の目標変位refzb,refzcは、(39)式,(40)式の湾曲角度θ1、旋回角度ζ1に、目標湾曲角度refθ1、目標旋回角度refζ1を代入することで得られる。
(第7の実施形態)
次に、本発明の第7の実施形態について説明する。なお、以下に記載する第7の実施形態の説明では、上述した第1~第6の実施形態と共通する事項については説明を省略し、上述した第1~第6の実施形態と異なる事項について説明を行う。
次に、本発明の第7の実施形態について説明する。なお、以下に記載する第7の実施形態の説明では、上述した第1~第6の実施形態と共通する事項については説明を省略し、上述した第1~第6の実施形態と異なる事項について説明を行う。
上述した第1~第6の実施形態では、単一の湾曲可能部を有する連続体ロボット100のバックドライバビリティを向上する制御について説明した。本実施形態では、複数の湾曲可能部を有する連続体ロボットの制御系設計方法を説明する。
図27は、本発明の第7の実施形態に係る連続体ロボット100の概略構成の第2例を示す図である。以下、図27に示す連続体ロボット100を「連続体ロボット100-4」として記載する。この図27では、3つの湾曲可能部1100-1~1100-3が示されている。
ここでは、第nの湾曲可能部を駆動するワイヤの駆動変位をlpna,lpnb,lpncとし、複数の湾曲可能部1100を有するの連続体ロボット100-4のaワイヤ,bワイヤ,cワイヤの駆動変位lpna,lpnb,lpncと第nの湾曲可能部の遠位端の湾曲角度θn及び旋回角度ζnとの関係を求める。湾曲可能部の数をeとし、第nの湾曲可能部を駆動するワイヤの位相角を以下の(41)式とする。
これを用いて、第6の実施形態と同様に、各ワイヤを二重ループ制御系により駆動する。また、各湾曲可能部の間のバックドライブによる衝突を防ぐには、第6の実施形態と同様に、例えば各湾曲可能部のaワイヤを固定し、連続体ロボット100-4の中心軸方向のバックドライバビリティを0にすればよい。このときのbワイヤの駆動変位lpnb,cワイヤの駆動変位lpncは、それぞれ、以下の(43)式及び(44)式となる。
そして、第7の実施形態では、上述した第6の実施形態と同様に、各湾曲可能部1100のbワイヤ,cワイヤを二重ループ制御系により駆動すれば、湾曲方向のみにバックドライバビリティが得られる。
(その他の実施形態)
本発明は、上述の実施形態の1以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける1つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、1以上の機能を実現する回路(例えば、ASIC)によっても実現可能である。
このプログラム及び当該プログラムを記憶したコンピュータ読み取り可能な記憶媒体は、本発明に含まれる。
また、本発明のその他の実施形態としては、連続体ロボットの制御装置200の機能構成部を、連続体ロボット100に組み込んだ形態も、本発明に適用可能である。
本発明は、上述の実施形態の1以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける1つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、1以上の機能を実現する回路(例えば、ASIC)によっても実現可能である。
このプログラム及び当該プログラムを記憶したコンピュータ読み取り可能な記憶媒体は、本発明に含まれる。
また、本発明のその他の実施形態としては、連続体ロボットの制御装置200の機能構成部を、連続体ロボット100に組み込んだ形態も、本発明に適用可能である。
なお、上述した本発明の実施形態は、いずれも本発明を実施するにあたっての具体化の例を示したものに過ぎず、これらによって本発明の技術的範囲が限定的に解釈されてはならないものである。即ち、本発明はその技術思想、又はその主要な特徴から逸脱することなく、様々な形で実施することができる。
10:連続体ロボットの制御システム、100:連続体ロボット、200:連続体ロボットの制御装置、210:運動学演算部、KSV:位置制御部、KF:力制御部、Pn:連続体ロボットの動的モデル、300:入力装置
Claims (14)
- ワイヤで駆動され、湾曲可能に構成された少なくとも1つの湾曲可能部と、前記ワイヤを駆動する駆動部とを備える連続体ロボットの制御システムであって、
前記駆動部による前記ワイヤの押し引き駆動の目標変位と、前記連続体ロボットから得られる前記ワイヤを把持するワイヤ把持機構の変位との誤差を補償する制御を行うものであって、前記ワイヤの目標張力を出力する位置制御部と、
前記位置制御部から出力される前記ワイヤの目標張力と、前記連続体ロボットから得られる前記ワイヤの張力との誤差を補償する制御を行う力制御部と、
を有し、
前記力制御部を含む第1のループ制御系と、前記力制御部および前記位置制御部を含む第2のループ制御系とが構成されていることを特徴とする連続体ロボットの制御システム。 - 前記湾曲可能部における目標湾曲角度の入力に基づいて運動学演算を行い、前記目標変位を出力する運動学演算部を更に有することを特徴とする請求項1に記載の連続体ロボットの制御システム。
- 前記力制御部のゲインに第1の係数を乗じで第1のゲインとするとともに、前記位置制御部のゲインに第2の係数を乗じで第2のゲインとし、
前記第2のゲインは、前記第1のゲインに応じて定められることを特徴とする請求項1または2に記載の連続体ロボットの制御システム。 - 前記湾曲可能部における目標湾曲角度に応じて前記第1の係数を変化させることを特徴とする請求項3に記載の連続体ロボットの制御システム。
- 前記連続体ロボットは、1つの前記湾曲可能部に複数の前記ワイヤが構成され、前記複数のワイヤのそれぞれを駆動する複数の前記駆動部が備えられており、
前記複数の駆動部における各駆動部に対応して、前記第1のループ制御系および前記第2のループ制御系が構成されていることを特徴とする請求項1乃至4のいずれか1項に記載の連続体ロボットの制御システム。 - 前記力制御部のゲインに第1の係数を乗じで第1のゲインとする際に、前記各駆動部ごとに前記第1の係数が異なることを特徴とする請求項5に記載の連続体ロボットの制御システム。
- 前記連続体ロボットは、前記1つの湾曲可能部に構成された前記複数のワイヤのうちの1つのワイヤが、基台部に固定されていることを特徴とする請求項5または6に記載の連続体ロボットの制御システム。
- 前記力制御部は、前記連続体ロボットの運動方程式に基づく伝達関数および当該力制御部の開ループ伝達関数を演算する処理と、前記開ループ伝達関数から安定余裕の演算をする処理とを反復し、当該力制御部のゲインを定めることを特徴とする請求項1乃至7のいずれか1項に記載の連続体ロボットの制御システム。
- 前記力制御部は、PID制御器とローパスフィルタとの直列結合から構成されていることを特徴とする請求項1乃至8のいずれか1項に記載の連続体ロボットの制御システム。
- 前記PID制御器の微分ゲインは、当該微分ゲインを設定する処理と、前記力制御部および前記連続体ロボットの運動方程式に基づく伝達関数を用いて開ループ伝達関数を演算する処理とを反復することによって、定められることを特徴とする請求項9に記載の連続体ロボットの制御システム。
- 前記力制御部は、PID制御器と位相進みフィルタとローパスフィルタとの直列結合から構成されていることを特徴とする請求項1乃至8のいずれか1項に記載の連続体ロボットの制御システム。
- 前記位相進みフィルタは、折点周波数を設定する処理と、前記力制御部および前記連続体ロボットの運動方程式に基づく伝達関数を用いて開ループ伝達関数を演算する処理とを反復することによって、定められることを特徴とする請求項11に記載の連続体ロボットの制御システム。
- ワイヤと、
湾曲可能に構成された少なくとも1つの湾曲可能部と、
前記ワイヤを駆動する駆動部と、
を備え、
請求項1乃至12のいずれか1項に記載の連続体ロボットの制御システムを備えた連続体ロボット。 - ワイヤで駆動され、湾曲可能に構成された少なくとも1つの湾曲可能部と、前記ワイヤを駆動する駆動部とを備える連続体ロボットの制御方法であって、
位置制御部によって、前記駆動部による前記ワイヤの押し引き駆動の目標変位と、前記連続体ロボットから得られる前記ワイヤを把持するワイヤ把持機構の変位との誤差を補償する制御を行い、前記ワイヤの目標張力を出力する位置制御ステップと、
力制御部によって、前記位置制御部から出力される前記ワイヤの目標張力と、前記連続体ロボットから得られる前記ワイヤの張力との誤差を補償する制御を行う力制御ステップと、
を有し、
前記力制御部を含む第1のループ制御系と、前記力制御部および前記位置制御部を含む第2のループ制御系とが構成されていることを特徴とする連続体ロボットの制御方法。
Priority Applications (5)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2020028813A JP7379208B2 (ja) | 2020-02-21 | 2020-02-21 | 連続体ロボットの制御システム及びその制御方法、並びに、連続体ロボット |
PCT/JP2021/006085 WO2021166995A1 (ja) | 2020-02-21 | 2021-02-18 | 連続体ロボットの制御システム及びその制御方法、並びに、連続体ロボット |
CN202180015981.5A CN115135465A (zh) | 2020-02-21 | 2021-02-18 | 用于控制连续体机器人的系统和方法以及连续体机器人 |
EP21756932.6A EP4101605A4 (en) | 2020-02-21 | 2021-02-18 | CONTROL SYSTEM FOR ROBOT CONTINUUM AND CONTROL METHOD THEREFOR AND ROBOT CONTINUUM |
US17/890,104 US20220402130A1 (en) | 2020-02-21 | 2022-08-17 | System and method for controlling continuum robot, and continuum robot |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2020028813A JP7379208B2 (ja) | 2020-02-21 | 2020-02-21 | 連続体ロボットの制御システム及びその制御方法、並びに、連続体ロボット |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2021133429A JP2021133429A (ja) | 2021-09-13 |
JP7379208B2 true JP7379208B2 (ja) | 2023-11-14 |
Family
ID=77392178
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2020028813A Active JP7379208B2 (ja) | 2020-02-21 | 2020-02-21 | 連続体ロボットの制御システム及びその制御方法、並びに、連続体ロボット |
Country Status (5)
Country | Link |
---|---|
US (1) | US20220402130A1 (ja) |
EP (1) | EP4101605A4 (ja) |
JP (1) | JP7379208B2 (ja) |
CN (1) | CN115135465A (ja) |
WO (1) | WO2021166995A1 (ja) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2023122886A (ja) * | 2022-02-24 | 2023-09-05 | キヤノン株式会社 | 操作システムおよび操作システムの制御方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2018523508A (ja) | 2015-06-30 | 2018-08-23 | キヤノン ユーエスエイ, インコーポレイテッドCanon U.S.A., Inc | マニピュレータを制御する方法および装置 |
JP2019166307A (ja) | 2018-03-23 | 2019-10-03 | キヤノン株式会社 | 連続体ロボット制御装置、連続体ロボット制御方法及びプログラム |
JP2019202137A (ja) | 2018-05-17 | 2019-11-28 | キヤノン株式会社 | 連続体ロボットおよび連続体ロボットの制御システム |
Family Cites Families (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2770982B2 (ja) * | 1989-05-25 | 1998-07-02 | 株式会社豊田中央研究所 | マニピユレータの位置と力の協調制御装置 |
JPH0631662A (ja) * | 1992-07-14 | 1994-02-08 | Kobe Steel Ltd | マニピュレータの制御方法とその装置 |
JP2895352B2 (ja) * | 1993-06-30 | 1999-05-24 | 三菱電機株式会社 | 力制御機構 |
JP2011031354A (ja) * | 2009-08-04 | 2011-02-17 | Olympus Corp | アクチュエータ装置 |
US9539726B2 (en) * | 2012-04-20 | 2017-01-10 | Vanderbilt University | Systems and methods for safe compliant insertion and hybrid force/motion telemanipulation of continuum robots |
JP6169049B2 (ja) * | 2014-06-19 | 2017-07-26 | オリンパス株式会社 | マニピュレータの制御方法、マニピュレータ、およびマニピュレータシステム |
JP6865520B2 (ja) | 2014-06-24 | 2021-04-28 | 三菱ケミカル株式会社 | 菌体付着対策方法 |
US10434644B2 (en) * | 2014-11-03 | 2019-10-08 | The Board Of Trustees Of The Leland Stanford Junior University | Position/force control of a flexible manipulator under model-less control |
US11007641B2 (en) * | 2017-07-17 | 2021-05-18 | Canon U.S.A., Inc. | Continuum robot control methods and apparatus |
-
2020
- 2020-02-21 JP JP2020028813A patent/JP7379208B2/ja active Active
-
2021
- 2021-02-18 CN CN202180015981.5A patent/CN115135465A/zh active Pending
- 2021-02-18 WO PCT/JP2021/006085 patent/WO2021166995A1/ja unknown
- 2021-02-18 EP EP21756932.6A patent/EP4101605A4/en active Pending
-
2022
- 2022-08-17 US US17/890,104 patent/US20220402130A1/en active Pending
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2018523508A (ja) | 2015-06-30 | 2018-08-23 | キヤノン ユーエスエイ, インコーポレイテッドCanon U.S.A., Inc | マニピュレータを制御する方法および装置 |
JP2019166307A (ja) | 2018-03-23 | 2019-10-03 | キヤノン株式会社 | 連続体ロボット制御装置、連続体ロボット制御方法及びプログラム |
JP2019202137A (ja) | 2018-05-17 | 2019-11-28 | キヤノン株式会社 | 連続体ロボットおよび連続体ロボットの制御システム |
Also Published As
Publication number | Publication date |
---|---|
WO2021166995A1 (ja) | 2021-08-26 |
CN115135465A (zh) | 2022-09-30 |
US20220402130A1 (en) | 2022-12-22 |
EP4101605A4 (en) | 2024-03-13 |
EP4101605A1 (en) | 2022-12-14 |
JP2021133429A (ja) | 2021-09-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11877814B2 (en) | Tension control in actuation of multi-joint medical instruments | |
Dalvand et al. | An analytical loading model for $ n $-tendon continuum robots | |
WO2019221036A1 (ja) | 連続体ロボットおよび連続体ロボットの制御システム | |
JP2018008335A (ja) | 連続体ロボット、その運動学の補正方法、および連続体ロボットの制御方法 | |
US9364289B2 (en) | Interleaved manipulator | |
JP2019166307A (ja) | 連続体ロボット制御装置、連続体ロボット制御方法及びプログラム | |
JP7379208B2 (ja) | 連続体ロボットの制御システム及びその制御方法、並びに、連続体ロボット | |
Suh et al. | Harmonious cable actuation mechanism for soft robot joints using a pair of noncircular pulleys | |
Zhao et al. | Continuum manipulator with redundant backbones and constrained bending curvature for continuously variable stiffness | |
Widmann et al. | Control of industrial robots with flexible joints | |
US20150080908A1 (en) | Energy balance mechanism for flexure joint | |
WO2019181694A1 (en) | Continuum robot control device, continuum robot control method, and program | |
Ahmed et al. | Kinestatic modeling of a spatial screw-driven continuum robot | |
Carusone et al. | Tracking control for end‐effector position and orientation of structurally flexible manipulators | |
Andeen et al. | Design of compliance in robots | |
WO2023022056A1 (ja) | 連続体ロボットの制御システム及びその制御方法 | |
WO1994017461A1 (en) | Apparatus for controlling weaving of robot | |
Li et al. | Optimization design method of tendon-sheath transmission path under curvature constraint | |
Jeong et al. | Stiffness analysis and experimental validation of modular-type hybrid antagonistic tendon-driven joint systems | |
WO1984004722A1 (en) | Articulated robot | |
Vakil et al. | Piece-wise causal inversion by output redefinition for a flexible link manipulator | |
JP2014104517A (ja) | ロボットシステム | |
Kanarachos et al. | Nonlinear vibration control of a flexible-link robot arm based on a Taylor series expansion of the control law | |
Jeong et al. | Control strategy of slack enabling tendon actuator for the soft wearable robot using feedback linearization | |
Lim et al. | Anti-vibration PID control for a robot manipulator experiments |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20230116 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20231003 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20231101 |
|
R151 | Written notification of patent or utility model registration |
Ref document number: 7379208 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |