TOP > 国内特許検索 > 能動連結機構及びその位置姿勢の検出方法及びその制御方法 > 明細書

明細書 :能動連結機構及びその位置姿勢の検出方法及びその制御方法

発行国 日本国特許庁(JP)
公報種別 特許公報(B2)
特許番号 特許第4413251号 (P4413251)
公開番号 特開2009-030748 (P2009-030748A)
登録日 平成21年11月27日(2009.11.27)
発行日 平成22年2月10日(2010.2.10)
公開日 平成21年2月12日(2009.2.12)
発明の名称または考案の名称 能動連結機構及びその位置姿勢の検出方法及びその制御方法
国際特許分類 F16C  11/04        (2006.01)
B60D   1/14        (2006.01)
B62D  55/065       (2006.01)
B62D  57/024       (2006.01)
B60D   1/26        (2006.01)
F16D   3/00        (2006.01)
A62B  99/00        (2009.01)
FI F16C 11/04 V
B60D 1/14 Z
B62D 55/065
B62D 57/02 J
B60D 1/10
F16D 3/00
A62B 37/00 Z
請求項の数または発明の数 4
全頁数 13
出願番号 特願2007-196835 (P2007-196835)
出願日 平成19年7月30日(2007.7.30)
審査請求日 平成19年7月30日(2007.7.30)
特許権者または実用新案権者 【識別番号】507255916
【氏名又は名称】鄭 心知
【識別番号】591148093
【氏名又は名称】財団法人京都高度技術研究所
【識別番号】503132280
【氏名又は名称】特定非営利活動法人 国際レスキューシステム研究機構
発明者または考案者 【氏名】鄭 心知
個別代理人の代理人 【識別番号】100068087、【弁理士】、【氏名又は名称】森本 義弘
【識別番号】100096437、【弁理士】、【氏名又は名称】笹原 敏司
【識別番号】100100000、【弁理士】、【氏名又は名称】原田 洋平
審査官 【審査官】瀬川 裕
参考文献・文献 特開2004-350889(JP,A)
特開昭62-075119(JP,A)
特開2001-146950(JP,A)
特開平09-019508(JP,A)
調査した分野 F16C 11/04
B60D 1/14
B60D 1/26
B62D 55/065
B62D 57/024
F16D 3/00
A62B 99/00
特許請求の範囲 【請求項1】
先行する第1ユニットと後行する第2ユニットとを連結し、第2ユニットから第1ユニットへ駆動力を伝達する能動連結機構において、
この能動連結機構は、一対の関節駆動軸と推進駆動軸からなり、前記各軸の中央部が三次元ジョイントにより接合され、
前記関節駆動軸は、両端部に互いに逆向きのネジが形成されて回動可能に軸支され、回動により三次元ジョイントで接合された関節駆動軸の各軸要素が各々のユニットから対称に出退して、各ユニット間の関節駆動軸の長さを可変とし、
推進駆動軸は、両端が各ユニットに回転可能に軸支されると共に、各ユニット間の長さは固定され、中央部の三次元ジョイントを中心として第1ユニットを操作する、ことを特徴とする能動連結機構。
【請求項2】
一対の関節駆動軸と推進駆動軸が逆2等辺三角形に配置されたことを特徴とする請求項1に記載の能動連結機構。
【請求項3】
中央部が三次元ジョイントにより接合された一対の関節駆動軸と推進駆動軸からなり、先行する第1ユニットと後行する第2ユニットとを連結し、第2ユニットから第1ユニットへ駆動力を伝達する能動連結機構の位置姿勢の検出方法において、
前記一対の関節駆動軸の寸法を検出し、この検出した関節駆動軸の寸法から、推進駆動軸の三次元ジョイントの関節角を求めた後、第1ユニットの先端位置姿勢を求めることを特徴とする能動連結機構の位置姿勢の検出方法。
【請求項4】
中央部が三次元ジョイントにより接合された一対の関節駆動軸と推進駆動軸からなり、先行する第1ユニットと後行する第2ユニットとを連結し、第2ユニットから第1ユニットへ駆動力を伝達する能動連結機構の制御方法において、
所望の第1ユニットの先端位置姿勢を決定し、この先端位置姿勢の実現に必要な推進駆動軸の関節角を求め、この推進駆動軸の三次元ジョイントの関節角から関節駆動軸の変位寸法量を求め、この変位寸法量に一対の関節駆動軸の寸法を適合させる操作を行うことを特徴とする能動連結機構の制御方法。
発明の詳細な説明 【技術分野】
【0001】
本発明は、先進する第1ユニットに、連結する後方の第2ユニットから駆動力を伝達する能動連結機構及びその位置姿勢の状態を検出すると共に、第1ユニットを所望の姿勢に制御するその制御方法に関し、本発明は、種々の装置に応用できるが、その応用例の一例として震災や災害時に建造物等の瓦礫の中を探査するための探査用装置や原子力発電所等の危険な領域での監視システム等に応用することのできる能動連結機構及びその位置姿勢の検出方法及びその制御方法に関する。
【背景技術】
【0002】
前記のように、震災や災害時に建造物等の瓦礫の中を探査する際、生存者の救出のために、種々の探査用装置が開発されている。その一例として特開2004-350889号公報に簡易型探査用レスキュー装置が掲載されている。この特許文献記載のレスキュー装置は先端部に車輪を有する探査部を備え、駆動ハンドルから駆動用ワイヤを介して車輪に駆動力を伝達している。さらに、2本の操舵用ワイヤがケーブル部内に配置され、その先端は前記探査部の背面側の左右両側にそれぞれ1本ずつ接続され、他端には操舵用ハンドルが接続されている。操縦者はケーブル部の後端部を把持しながら、操舵用ハンドルを牽引することにより探査部の方向を左右に操縦している。

【特許文献1】特開2004-350889号公報
【発明の開示】
【発明が解決しようとする課題】
【0003】
しかしながら、前記従来の探査用レスキュー装置の探査部は瓦礫の中に完全に入り込んで隠れた状態にあることもあり、先端の探査部の姿勢が左右方向、上下方向にどれだけが傾いているのかその姿勢を正確につかむことができず、カメラに写し出された映像もどのような角度で写し出されているのか判然としない。しかも、現在の姿勢から所望の角度だけ探査部の角度を変えようとしても探査部が目視できないため、不可能であった。かりに、探査部の姿勢を検知するとすれば、探査部の角度を検出する角度検出センサーを探査部の外部等に装備する必要がある。さらに、2本の操舵用ワイヤを牽引しても探査部は左右方向の操縦しかできず、探査部を上方向に持上げて障害物を乗り越えるようにするためには、さらに操舵用ワイヤを1本増やさなければならない。また、狭隘な瓦礫等の中に入り込む場合、探査部の小型化の必要性、それによる探査部の搭載スペースの制約によって、探査部と同一のユニット上に動力機構が搭載できず、何らかの形で連結する別の動力発生ユニットから駆動に必要な動力を得る必要がある。
【課題を解決するための手段】
【0004】
本発明は上記課題を解決するためになされたもので、機構上の最少構成を有しながら、後方動力ユニットから先方駆動ユニットへ駆動力を伝達するとともに、目視することのできない先行する第1ユニットの現在の位置姿勢を検出することを可能とし、しかもその第1ユニットの現在の姿勢から所望の角度の姿勢に変更することができる能動連結機構及びその位置姿勢の検出方法及びその制御方法を提供することを目的とするものである。
【0005】
本発明の能動連結機構は、先行する第1ユニットと後行する第2ユニットとを連結し、第2ユニットから第1ユニットへ駆動力を伝達する能動連結機構であって、この能動連結機構は、一対の関節駆動軸と推進駆動軸からなり、前記各軸の中央部が三次元ジョイントにより接合され、前記関節駆動軸は、両端部に互いに逆向きのネジが形成されて回動可能に軸支され、回動により三次元ジョイントで接合された関節駆動軸の各軸要素が各々のユニットから対称に出退して、各ユニット間の関節駆動軸の長さを可変とし、推進駆動軸は、両端が各ユニットに回転可能に軸支されると共に、各ユニット間の長さは固定され、中央部の三次元ジョイントを中心として第1ユニットを操作することを特徴とする。
【0006】
また、本発明の能動連結機構は、一対の関節駆動軸と推進駆動軸が逆2等辺三角形に配置されたことを特徴とする。
本発明の上記構成により、一対の関節駆動軸の寸法を検出することにより、推進駆動軸の三次元ジョイントの関節角を求め、さらに第1ユニットの先端位置姿勢を求めることができる。
【0007】
さらに、本発明の上記構成により、先端位置姿勢の実現に必要な推進駆動軸の関節角を求め、この推進駆動軸の三次元ジョイントの関節角から関節駆動軸の寸法を求め、この関節駆動軸寸法に一対の関節駆動軸を適合させることにより、第1ユニットを所望の先端位置姿勢にすることができる。
【0008】
さらに、本発明の上記構成により、第1ユニット先端位置姿勢の検出と制御が容易にできるともに、前記各軸の中央部が前記三次元ジョイントにより結合されていても、本発明の能動連結機構で連結される第1ユニットと第2ユニットとの間は各駆動軸線を中心に相捻れるような運動の自由度が発生することはなく、各駆動軸により第2ユニットから第1ユニットへ駆動力を伝達することができる。
【0009】
さらに、本発明の能動連結機構は、動作途中のどの状態においても、特異姿勢となることがない。即ち、一対の関節駆動軸と推進駆動軸全体での自由度が縮退して第1ユニットの動きが取れなくなることはない。さらに、一対の関節駆動軸と推進駆動軸全体で所望外の自由度が発生して第1ユニットを所定の姿勢で固定できなくなることはない。
また、本発明の能動連結機構の位置姿勢の検出方法は、中央部が三次元ジョイントにより接合された一対の関節駆動軸と推進駆動軸からなり、先行する第1ユニットと後行する第2ユニットとを連結し、第2ユニットから第1ユニットへ駆動力を伝達する能動連結機構の位置姿勢の検出方法において、前記一対の関節駆動軸の寸法を検出し、この検出した関節駆動軸の寸法から、推進駆動軸の三次元ジョイントの関節角を求めた後、第1ユニットの先端位置姿勢を求めることを特徴とする。
【0010】
また、本発明の能動連結機構の制御方法は、中央部が三次元ジョイントにより接合された一対の関節駆動軸と推進駆動軸からなり、先行する第1ユニットと後行する第2ユニットとを連結し、第2ユニットから第1ユニットへ駆動力を伝達する能動連結機構の制御方法において、所望の第1ユニットの先端位置姿勢を決定し、この先端位置姿勢の実現に必要な推進駆動軸の関節角を求め、この推進駆動軸の三次元ジョイントの関節角から関節駆動軸の変位寸法量を求め、この関節駆動軸の変位寸法量に一対の関節駆動軸の寸法を適合させる操作を行うことを特徴とする。
【発明の効果】
【0011】
本発明の上記構成により、先行する第1ユニットで必要な駆動力をその後方に連結される第2ユニットから伝達することができる。さらに、目視できない先行する第1ユニットの姿勢を、一対の関節駆動軸の寸法から正確に割り出すことができる。さらに、目視できない第1ユニットを所望の姿勢に制御することができる。
【発明を実施するための最良の形態】
【0012】
以下に、本発明を実施するための最良の形態について、図面を用いて説明する。
図1は、本発明の動力伝達機能を有する能動連結機構の概略斜視図である。この能動連結機構は、先行する第1ユニット1と、この第1ユニット1の後方に接合する第2ユニット2との間に配置されて、第2ユニット2の後端に接続されたケーブル3を介して制御装置を通して第1ユニット1の位置姿勢を検出したり、制御するものである。
【0013】
図1には図示していないが、第1ユニット1には、例えばCCDカメラや温度や湿度等を測定するセンサーが搭載されている。ただし、この第1ユニット1には、電源(バッテリ)や自走するための駆動装置(具体的にはモータを含めた駆動機構)は搭載されていない。駆動力は第2ユニット2から伝達される。また、電力はケーブル3を通して送られている。第1ユニット1の外壁面には無限軌道(クローラ)4が取り付けられており、この無限軌道4により第1ユニット1が移動し、第2ユニットがそれに牽引されて移動できるように構成されている。なお、この図1では原理だけを説明するため、詳細な構成は省略してある。なお、無限軌道4は後述する推進駆動軸5からの駆動力に基づいて駆動するものである。
【0014】
第2ユニット2には、関節駆動装置6(以下、関節駆動モータという)、推進駆動装置7(以下、推進駆動モータという)、関節駆動モータ6又は関節駆動軸8、9の回転数をカウントするエンコーダ10(EN)等が備えられており、その他、第1ユニット1を支援するための装置が搭載されている。関節駆動モータ6は後述するように左右の関節駆動軸8、9を回転して第1ユニット1の姿勢を変えるための駆動力を発生させるためのモータである。推進駆動モータ7は第1ユニット1の無限軌道4に回転駆動力を与えるためのモータである。なお、ケーブル3は制御装置とユニット1、2間の信号のやり取りを行うと共に、第1ユニット1と第2ユニット2の電力を送電するためのケーブルである。
【0015】
次に、能動連結機構の構成について説明する。能動連結機構は左右一対の関節駆動軸8、9と、1本の推進駆動軸5から構成されている。関節駆動軸8、9は第1ユニット1の姿勢を制御するものであり、推進駆動軸5は、前記のように第1ユニット1に回転駆動力を伝達するものである。この2本の関節駆動軸8、9と1本の推進駆動軸5との配置は、関節駆動軸8、9が上部に平行に所定間隔を開けて配置されており、この2本の関節駆動軸8、9に対して逆2等辺三角形となる位置に推進駆動軸5が配置されている。
関節駆動軸8、9は、図2に示すように、2本の軸要素8a、8b及び軸要素9a,9bが中央部で三次元ジョイント11、12(ユニバーサルジョイント、自在継手、万能継手等で呼ばれる三次元方向に曲がる継ぎ手を言う。またスプリングのようなものであってもよい。以下、図示するユニバーサルジョイントの用語を用いて説明する)により連結され、このユニバーサルジョイント11、12を挟んでそれぞれの軸要素8a、8b、9a、9bを上下、左右、斜め方向に曲げることができる。関節駆動軸8、9の両端部には、それぞれ逆方向の右ネジ15と左ネジ14が所定の長さだけ同ピッチで雄ネジとして刻まれている。さらに、この右ネジ15と左ネジ14に、それぞれ雌ネジ部材16(ナット)が螺合している。これらの雌ネジ部材16はそれぞれの第1ユニット1、第2ユニット2の基台17、18に固定されて移動は不可能である。これに対して、関節駆動軸8、9は、雌ネジ部材16に螺合した右ネジ15と左ネジ14によって、ユニバーサルジョイント11、12を挟んで連結している2本の軸要素8a、8b、9a、9bが、関節駆動軸8、9の正逆方向の回転により、対称(ユニバーサルジョイントを挟んで逆方向)に移動する。このため、ユニバーサルジョイント11、12を挟んで接合した各2本の軸要素8a、8b、9a、9bが同じ長さだけ第1ユニット1と第2ユニッオ2から出入りして両雌ネジ部材16、16間の関節駆動軸8、9の全体の長さが伸縮する。このユニバーサルジョイント11、12をはさんでそれぞれの軸要素8a、8b、9a、9bがそれぞれのユニット1,2から出入りすることを「関節対称駆動」という。これについてはさらに詳細に説明する。
【0016】
第2ユニット2側の関節駆動軸8、9の一端には、関節駆動モータ6が取り付けられている。この関節駆動モータ6は正逆に回転することが可能でなければならず、しかも前記関節駆動軸8、9を回転して第1ユニット1の姿勢を操作するに十分な回転力(トルク)を備えたモータを選定しなければならない。この関節駆動モータ6は、関節駆動軸8、9の回転に伴って同じように回転してはならないが、この関節駆動モータ6の回転軸の軸芯方向にスライド可能な構造(図示せず)でなければならない。
【0017】
関節駆動軸8、9に近接して、この駆動軸8、9の回転数をカウントするエンコーダ10(EN)が備え付けられている。図2のエンコーダは関節駆動軸8,9のマーカをエンコーダ10の検知器がカウントして関節駆動軸8、9の回転数を検知するようにしているが、このエンコーダ10は他の構成であってもよく、例えば、関節駆動モータ6の回転数を検知するようなものであってもよい。このエンコーダ10による関節駆動軸8、9の回転数の検出により、ユニット間(雌ネジ部材16間)の関節駆動軸8、9の全長が判明する。
【0018】
推進駆動軸5は、第1ユニット1の無限軌道4を駆動する駆動機構(図示せず)に推進駆動モータ7の回転力を伝達するための駆動軸である。この推進駆動軸5は2本の軸要素5a、5bが中央部でユニバーサルジョイント13により連結され、このユニバーサルジョイント13をはさんでそれぞれ上下、左右、斜め方向に曲げることができる。しかし、この推進駆動軸5は前記の関節駆動軸8、9と異なり両端部に左右の逆ネジは形成されておらず、回転は可能であるが、正逆方向の回動に伴って軸支された位置が変更することがない。即ち、前記関節駆動軸8、9のように正逆方向の回転により、第1ユニット1と第2ユニット2から軸要素5a、5bが対称に出入りすることがなく、回動は可能であるが軸芯方向に移動することなく、軸支された状態を維持する。従って、この推進駆動軸5の寸法により第1ユニット1と第2ユニット2との間隔が定まり、しかもユニバーサルジョイント13がヒンジの役割を果たし、ユニバーサルジョイント13を中心にして第2ユニットに対して第1ユニット1が回動することができる。
【0019】
第2ユニット2側の前記推進駆動軸5の端部に推進駆動モータ7が接続している。推進駆動モータ7は前記のように第1ユニット1の無限軌道4を駆動するに十分な回転力(トルク)を出力できるものを選択しなければならない。この推進駆動モータ7は第2ユニット2の基台18に固定されており、関節駆動モータ6のように、推進駆動モータ7の回転軸の軸芯方向にスライドしながら回転することはない。従って、前記のように推進駆動軸5も軸芯方向に動くことはない。第1ユニット1側の推進駆動軸5の端部は軸受(図示せず)に軸支されている。この軸支の構成も推進駆動軸5が、軸芯方向に移動することが出来ないように、固定されている。
【0020】
次に、前記構成の作動原理について説明する。
図3、4は本件発明の能動連結機構の作動原理を説明するための説明図である。この図面は第1ユニットを円筒状に簡略化して描いている。これらの図において、前記の説明と同様の部材は同一の符号を付してある。また、第1ユニットが回動する基準軸は図1に示す、Y軸、X軸、Z軸として説明する。
【0021】
本発明の能動連結機構は、第1ユニット1が瓦礫等の中を探査していて、その姿勢等が不明であっても、「順運動学関係」から、関節駆動軸8、9の変位寸法量d、d(数式上、関節駆動軸変数と言うことがある)及びそれから導きだされる関節角θ、θから第1ユニット1の位置、姿勢を解析することが可能となる。また、「逆運動学関係」から、第1ユニット1の目標位置の条件を設定すると、それを満たす関節角θ、θ及び関節駆動軸8、9の変位寸法量d、dを求めて第1ユニット1を所望の姿勢となるように操作することができる。以下、これらの点について説明する。
I)第1ユニットの先端位置姿勢を求める。(順運動学関係) この順運動学関係は、前記のように第1ユニット1の先端位置姿勢が不明であっても、関節駆動軸8、9の変位寸法量d、d(左右関節駆動軸変数)から、関節角θ、θを求め、さらに、第1ユニットのユニット先端位置姿勢を求めるものである。ユニット先端位置姿勢とは、第1ユニットの特定の位置(例えば、CCDカメラのレンズの位置)の座標軸の距離及び角度から第1ユニットの位置姿勢を表すものである。
【0022】
前記のdは、左側(left)の関節駆動軸8の変位寸法量、dは、右側(right)の関節駆動軸9の変位寸法量を示している。さらに変位寸法量d、dを図1、図2を用いて具体的に説明する。変位寸法量d、dは、折れ曲がった状態を含めて、両雌ネジ部材16,16間の関節駆動軸8、9の軸要素8a,8b,9a,9bの長さ(もしくは下記基準姿勢における基準寸法に対する変化量)であるが、第1ユニット1と、第2ユニット2間の関節駆動軸8,9の軸要素8a,8b,9a,9bの長さ(もしくは下記基準姿勢における基準寸法に対する変化量)と言い表すこともできる。この変位寸法量d、dは、前記の説明のとおり、関節駆動モータ6、6により、関節駆動軸8、9を正逆方向に回転することにより、両ユニット間の全長(以下、単に寸法という)は変化する。前記のように、図2の雌ネジ部材16,16に螺合している関節駆動軸8,9には右ネジ15と左ネジ14が刻まれているので、関節駆動モータ6、6の回転により、ユニバーサルジョイント11,12を挟んで連結されたそれぞれの軸要素8a,8b,9a,9bは、同寸法だけ対称(軸要素がそれぞれ逆方向)に両ユニット1,2から出入りする。第1ユニット1と第2ユニット2が、図3において、第1ユニットを点線で示すように一直線状の姿勢(以下、基準姿勢という)の場合の両ユニット1,2間の関節駆動軸8,9の寸法を基準寸法とすると、関節駆動モータ6,6の例えば正回転により軸要素8a,8b,9a,9bがそれぞれのユニット1,2に退いていく(中に入る状態)とすると、両ユニット1,2間の関節駆動軸8,9の寸法は、前記基準寸法より短くなる。逆に、関節駆動モータ6,6を逆回転すると、軸要素8a,8b,9a,9bはそれぞれのユニット1,2から同じ寸法だけ出てくる(中から出てくる状態)ので、両ユニット1,2間の関節駆動軸8,9の寸法は前記の基準寸法より長くなる。この両ユニット1,2間の関節駆動軸8,9の軸要素8a,8b,9a,9bの伸縮する寸法を変位寸法量d、dと言う。この変位寸法量d、dは、第1ユニットの先端位置姿勢を求める際に、数式を解析する場合は左右の関節駆動軸の左右関節駆動軸変数と言う。
この関節駆動軸8,9の左右関節駆動軸変数d、dから、後述する関数関係を解析することにより、関節角θ、θを求めることができる。関節角θは、図3に示すように、第1ユニット1が、上下方向に回転した場合の回転角である。より正確には、推進駆動軸5のユニバーサルジョイント13からY軸方向に垂直に伸びる軸線20(仮想Y軸基準線)が、変位寸法量d、dの変化に伴って、前記ユニバーサルジョイント13の位置におけるZ軸線を中心として回動した場合の回転角である。図3は、第1ユニット1がユニバーサルジョイント13のZ軸線を中心にθだけ回転して上方に持上げられた状態を示す正面図である。関節角θは、変位寸法量d、dの変化に伴って、第1ユニット1が、仮想Y軸基準線20を中心として、左右方向に回動した場合の回転角である。図4の平面図は、図3の正面図の状態から第1ユニット1がさらに仮想Y軸基準線20を中心にしてθだけ左方向に回転した状態を示している。
【0023】
次に、関節駆動軸の変位寸法量d、dと、関節角θ、θとの関係について説明する。
1)関節駆動軸要素8a,8bの変位寸法量dと、同9a,9bの変位寸法量dとが同じだけ基準寸法より短い状態。
【0024】
第1ユニット1が、基準姿勢の状態からユニバーサルジョイント13のZ軸線を中心に上方に曲がる。変位寸法量d、dの変化量が大きくなると、関節角θは大きくなり、変位寸法量d、dの変化量が小さくなると、関節角θは小さくなる。
2)関節駆動軸要素8a,8bの変位寸法量dと、同9a,9bが変位寸法量dとが同じだけ基準寸法より長い状態。
【0025】
第1ユニット1が、基準姿勢の状態からユニバーサルジョイント13のZ軸を中心に下方に曲がる。変位寸法量d、dの変化量が大きくなると、関節角-θの絶対値は大きくなり、変位寸法量d、dの変化量が小さくなると、関節角-θの絶対値は小さくなる。
3)基準寸法より、左側関節駆動軸要素8a,8bの変位寸法量dが短く、右側関節駆動軸要素9a,9bの変位寸法量dが長い状態。
【0026】
第1ユニット1が、基準姿勢の状態から仮想Y軸基準線20を中心に左に曲がる。変位寸法量d、dの変化量が大きくなると関節角θは大きくなり、変位寸法量d、dの変化量が小さくなると、関節角θは小さくなる。
4)基準寸法より、左側関節駆動軸要素8a,8bの変位寸法量dが長く、右側関節駆動軸要素9a,9bの変位寸法量dが短い状態。
【0027】
第1ユニット1が、基準姿勢の状態から仮想Y軸基準線20を中心に右に曲がる。変位寸法量d、dの変化量が小さくなると関節角-θの絶対値は小さくなり、変位寸法量d、dの変化量が大きくなると、関節角-θの絶対値は大きくなる。
【0028】
以上の1)2)3)4)の基本姿態をさらに組み合わせた、1)+3)、1)+4)、2)+3)、2)+4)の姿勢ができる。この姿勢は第1ユニット1がZ軸線を中心に上下方向に曲がった状態から、さらに、左右の何れかの方向に回転して捻れた状態の組み合わせである。
【0029】
上記の変位寸法量d、dと関節角θ、θとは幾何学的関係から次のような関数関係となる。
(d、d)=g(θ、θ) 式I
この式Iの逆関数は次の式IIのようになり、d、dより関節角θ、θを求める際に用いる順運動学関係である。
【0030】
、θ)=g-1(d、d) 式II
上記の式I、IIのgは関数を表し、そのパラメータ値は、能動連結機構の種々の寸法等の要素によって定まる。例えば、推進駆動軸5と関節駆動軸8,9の寸法及び関節駆動軸8,9と推進駆動軸5との距離関係等から導き出されるものである。(d、d)は前記のように関節駆動モータ6の回転数をエンコーダ10によりカウントすることにより判明する。左右関節駆動軸変数(d、d)よりgの逆関数(式II)から関節角θ、θを求めることができる。
【0031】
また、能動連結機構を表す数学モデルとして関節座標系(xi,yi,zi)と関節動作を表す関節角θ、θとを適正に設定することにより、基準座標系(xR,yR,zR)やユニット先端座標系(xE,yE,zE)を含めた座標系間の同時変換関係を表す行列
i-1ii-1ii)
の各要素を用いることにより、次のようなθ、θから第1ユニット先端位置姿勢までの順運動学関係を求めることができる。
【0032】
(xE,yE,zEEEE)=f(θ、θ) 式III
上記式IIIの(xE,yE,zE)は、第1ユニット1の先端位置(Eed)における先端座標系の基準座標系に対する距離を示し、(θEEE)は、先端座標系の基準座標系に対する角度を示している。この距離(xE,yE,zE)と角度(θEEE)により第1ユニットの先端位置姿勢を表している。
【0033】
また、式IIIのfは関数(function)を示し、ユニット1、2や能動連結機構の寸法等によって定まるものである。
以上のような式I、式II、式IIIを用いて、左右関節駆動軸変数(関節駆動軸の変位寸法量)d、dから、現在の第1ユニットの先端位置姿勢を求めることができる。
【0034】
次に、第1ユニット1の現在の先端位置姿勢をパーソナルコンピュータにより求める場合を図5のフローチャートと、図7のブロック図を用いて説明する。
図7のI/F,CPU,RAM,ROM,LCD(液晶ディスプレイ)等はパーソナルコンピュータを示しているが、制御用の特別なマイクロコンピュータを用いたものであってもよい。図7において、6、6は関節駆動モータ、7は推進駆動モータ、10,10はエンコーダ(EN)を示している。
【0035】
ステップ1;第1ユニット1の先端位置姿勢のn周期目の検出のスタートを示している。キーボードからの指令により、ROM内に格納されているプログラムにより先端位置姿勢の検出を開始する。
【0036】
ステップ2;左側の関節駆動軸8に設けられているエンコーダ10(L)から左側関節駆動軸8の変位寸法量(d)を検出する。即ち、左側のエンコーダ10(L)の正逆方向の回転数から関節駆動軸8の現在の寸法を割り出し変位寸法量(d)を検出する。
【0037】
ステップ3;右側の関節駆動軸9に設けられているエンコーダ10(R)から右側関節駆動軸9の変位寸法量(d)を検出する。即ち、右側のエンコーダ10(R)の正逆方向の回転数から関節駆動軸9の現在の寸法を割り出し変位寸法量(d)を検出する。
【0038】
ステップ4、5;前記ステップ2で検出した変位寸法量(d)と、前記ステップ3で検出した変位寸法量(d)とからZ軸回りの関節角θと、仮想Y軸基準線回りの関節角θとを算出する。この関節角θθを算出する式IIの関数関係は前記の通りである。
【0039】
ステップ6;前記ステップ4、5で求めた関節角θθから第1ユニット1の先端位置姿勢(xE,yE,zEEEE)を算出する。この第1ユニット1の先端位置姿勢は式IIIから求めることができることは、前記の通りである。
【0040】
ステップ7;前記のステップ6で求めた結果をLCDに表示する。第1ユニット1の先端位置姿勢(xE,yE,zEEEE)を出力して、第1ユニット1の先端位置姿勢を画像としてLCDに表示する。
【0041】
ステップ8;ステップ7により求めた第1ユニット1の先端位置姿勢からn+1周期目の検出に移行する。n+1周期目以降も同様にして刻々変化する第1ユニット1の先端位置姿勢を算出し、LCDに表示することができる。
II)第1ユニット先端目標位置実現のための変位寸法量を求める。(逆運動学関係)
逆運動学関係は、第1ユニット1の先端位置姿勢から関節駆動軸8,9の変位寸法量d、dへの関係を与えるものであり、操縦者が所望する第1ユニット1の先端位置姿勢を実現するために必要な関節駆動軸8,9の変位寸法量d、dを求めるのに用いる。即ち、第1ユニット1を目標とする位置姿勢にするために左右の関節駆動軸8,9の寸法(d、d)をどれだけにすればよいかを解析するものである。そのためには、第1ユニットの先端位置姿勢の実現に必要な回転角θd1、θd2を求め、さらに必要な関節駆動軸8,9の変位寸法量(左右関節駆動軸変数)ddl、ddrを求める逆運動学関係を解析的に解くことにより、第1ユニット1の先端目標位置姿勢を実現する。
【0042】
前記の順運動学関係の式IIIの逆関係である逆運動学関係は次の通り表されるとする。
、θ)=f-1(xE,yE,zEEEE) 式V
いま、第1ユニット1の先端目標位置姿勢を次のように与えられるとする。
【0043】
(xE,yE,zEEEE 式VI
この式VIに逆運動学関係式Vを当てはめると、目標回転角θd1、θd2が求められる。
【0044】
d1、θd2)=f-1(xE,yE,zEEEE
さらに、回転角θd1、θd2から関数式Iを用いて左右関節駆動軸変数(関節駆動軸の変位寸法量)ddl、ddrを求めることができる。
【0045】
(ddl、ddr)=g(θd1、θd2
このようにして求めた関節駆動軸の変位寸法量ddl、ddrに基づいて関節駆動モータ6を回動して回転駆動軸8,9の両ユニット1,2間の寸法をこの変位寸法量ddl、ddrに適合するように制御すれば、第1ユニット1の先端位置姿勢を実現することができる。
【0046】
次に、前記の説明のうち、第1ユニットの先端位置姿勢を実現するための制御方法について、図6のフローチャートと、図7のブロック図を用いて説明する。図7のI/F,CPU,RAM,ROM,LCD等はパーソナルコンピュータを示しているが、制御用の特別なコントローラであってもよい。
【0047】
先ず、前記のように、逆運動学関係により第1ユニット1の先端位置姿勢(xE,yE,zEEEEを実現するために必要な第1ユニット1の回転角θd1、θd2を導き出し、ユニバーサルジョイント13のZ軸線を中心軸として第1ユニット1が上下に回動する回転角θd1と、仮想Y軸基準線20を中心軸として第1ユニット1が左右に回動する回転角θd2に分解する。分解した回転角θd1のZ軸線を中心軸とする動作指令と、回転角θd2の仮想Y軸基準線20を中心軸とする動作指令を出す。(ステップ1)
次に、(ステップ1)で分解した回転角θd1から左側関節駆動軸8の関節駆動変位寸法量ddl 1、と右側関節駆動軸9の関節駆動変位寸法量ddr 1とに分解する。(ステップ2)
さらに、(ステップ1)で分解した回転角θd2から左側関節駆動軸8の関節駆動変位寸法量ddl 2、と右側関節駆動軸9の関節駆動変位寸法量ddr 2とに分解する。(ステップ3)
次に、ステップ2とステップ3で分解した関節駆動変位寸法量ddl 1、関節駆動変位寸法量ddr1、関節駆動変位寸法量ddl 2、関節駆動変位寸法量ddr 2を用いて、ddl 1とddl 2とを足し合わせて左側関節駆動軸8の目標変位寸法量ddlを得(ステップ4)、ddr1 とddr 2とを足し合わせて右側関節駆動軸9の目標変位寸法量ddrを得(ステップ5)、それらをサーボ制御により実現する。
【0048】
図6の左端のシーケンスは、左側関節駆動軸8の目標変位寸法量ddl を実現するためのサーボ制御をデジタルコンピュータを用いて離散的に実現する場合を示している。このサーボ制御(S.C.I)はステップ4によって得られる左側関節駆動軸8の目標変位寸法量ddlと、現在の左側の関節駆動軸8の変位寸法量dを比較する。左側関節駆動軸8の目標変位寸法量ddlと、現在の左側の関節駆動軸8の変位寸法量dとの差が0の場合、即ち左側関節駆動軸8の変位寸法量dを変える必要がない場合は現在のdを保持し次ぎの制御周期に移る。
もし、左側関節駆動軸の目標変位寸法量ddlと、現在の左側の関節駆動軸8の変位寸法量d間に誤差がある場合は、その誤差を修正する制御を行う。即ち、エンコーダ10(EN)により関節駆動軸8の回転数をカウントして、得られる現在の変位寸法量dを、ステップ4で得られる左側関節駆動軸の目標変位寸法量ddlと比較する。そして、その差に対してゲイン(誤差の変換・増幅率)Cの定めに基づき増幅し、さらにドライバによりモータ駆動入力に増幅し、左側のアクチュエータ(関節駆動モータ6)に通電し、左側関節駆動軸の目標変位寸法量ddlと、現在の左側関節駆動軸の変位寸法量dとの間の差が0となる方向に回転する。このようなサーボ制御により、左側関節駆動軸の目標変位寸法量ddlと、現在の左側関節駆動軸の変位寸法量dの誤差を0にする。
シーケンス(S.C.I)と同時に右隣りの第2の制御シーケンス(S.C.II)を同様に実施して、ステップ5で得られる右側関節駆動軸の目標変位寸法量ddrと、現在の右側の関節駆動軸の変位寸法量dを比較して、誤差がある場合はアクチュエータにより修正して誤差をなくすよう制御を行う。
【産業上の利用可能性】
【0049】
本発明の応用例の一例として震災や災害時に建造物等の瓦礫の中を探査するための探査用装置に応用することができる。
【図面の簡単な説明】
【0050】
【図1】本発明の能動連結機構の斜視図である。
【図2】本発明の能動連結機構の部分構成図である。
【図3】本発明の能動連結機構の作動原理を説明するための説明図である。
【図4】本発明の能動連結機構の作動原理を説明するための説明図である。
【図5】本発明の能動連結機構の位置姿勢の検出方法を示すフローチャートである。
【図6】本発明の能動連結機構の制御方法を示すフローチャートである。
【図7】本発明の能動連結機構の検出部・制御部等のブロック図である。
【符号の説明】
【0051】
1 第1ユニット
2 第2ユニット
3 ケーブル
4 無限軌道
5 推進駆動軸
6 関節駆動装置
7 推進駆動装置
8 関節駆動軸
8a 軸要素
8b 軸要素
9 関節駆動軸
9a 軸要素
9b 軸要素
10 エンコーダ
11 ユニバーサルジョイント
12 ユニバーサルジョイント
13 ユニバーサルジョイント
14 左ネジ
15 右ネジ
16 雌ネジ部材
17 基台
18 基台
20 仮想Y軸基準線
図面
【図1】
0
【図2】
1
【図3】
2
【図4】
3
【図5】
4
【図6】
5
【図7】
6