能動連結機構及びその位置姿勢の検出方法及びその制御方法
【課題】本発明の能動連結機構は震災や災害時に建造物等の特に狭隘な瓦礫の中を探査するための探査用装置に応用することができる。
【解決手段】能動連結機構が、一対の関節駆動軸と推進駆動軸からなり、前記各軸の中央部がユニバーサルジョイントにより接合され、前記関節駆動軸は、両端部に互いに逆向きのネジが形成されて回動可能に軸支され、回動によりユニバーサルジョイントで接合された関節駆動軸の各軸要素が各々のユニットから対称に出退して、各ユニット間の関節駆動軸の長さを可変とし、推進駆動軸は、両端が各ユニットに回転可能に軸支されると共に、各ユニット間の長さは固定され、中央部のユニバーサルジョイントを中心として第１ユニットが回動する。
【解決手段】能動連結機構が、一対の関節駆動軸と推進駆動軸からなり、前記各軸の中央部がユニバーサルジョイントにより接合され、前記関節駆動軸は、両端部に互いに逆向きのネジが形成されて回動可能に軸支され、回動によりユニバーサルジョイントで接合された関節駆動軸の各軸要素が各々のユニットから対称に出退して、各ユニット間の関節駆動軸の長さを可変とし、推進駆動軸は、両端が各ユニットに回転可能に軸支されると共に、各ユニット間の長さは固定され、中央部のユニバーサルジョイントを中心として第１ユニットが回動する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、先進する第１ユニットに、連結する後方の第２ユニットから駆動力を伝達する能動連結機構及びその位置姿勢の状態を検出すると共に、第１ユニットを所望の姿勢に制御するその制御方法に関し、本発明は、種々の装置に応用できるが、その応用例の一例として震災や災害時に建造物等の瓦礫の中を探査するための探査用装置や原子力発電所等の危険な領域での監視システム等に応用することのできる能動連結機構及びその位置姿勢の検出方法及びその制御方法に関する。
【背景技術】
【０００２】
前記のように、震災や災害時に建造物等の瓦礫の中を探査する際、生存者の救出のために、種々の探査用装置が開発されている。その一例として特開２００４−３５０８８９号公報に簡易型探査用レスキュー装置が掲載されている。この特許文献記載のレスキュー装置は先端部に車輪を有する探査部を備え、駆動ハンドルから駆動用ワイヤを介して車輪に駆動力を伝達している。さらに、２本の操舵用ワイヤがケーブル部内に配置され、その先端は前記探査部の背面側の左右両側にそれぞれ１本ずつ接続され、他端には操舵用ハンドルが接続されている。操縦者はケーブル部の後端部を把持しながら、操舵用ハンドルを牽引することにより探査部の方向を左右に操縦している。
【特許文献１】特開２００４−３５０８８９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、前記従来の探査用レスキュー装置の探査部は瓦礫の中に完全に入り込んで隠れた状態にあることもあり、先端の探査部の姿勢が左右方向、上下方向にどれだけが傾いているのかその姿勢を正確につかむことができず、カメラに写し出された映像もどのような角度で写し出されているのか判然としない。しかも、現在の姿勢から所望の角度だけ探査部の角度を変えようとしても探査部が目視できないため、不可能であった。かりに、探査部の姿勢を検知するとすれば、探査部の角度を検出する角度検出センサーを探査部の外部等に装備する必要がある。さらに、２本の操舵用ワイヤを牽引しても探査部は左右方向の操縦しかできず、探査部を上方向に持上げて障害物を乗り越えるようにするためには、さらに操舵用ワイヤを１本増やさなければならない。また、狭隘な瓦礫等の中に入り込む場合、探査部の小型化の必要性、それによる探査部の搭載スペースの制約によって、探査部と同一のユニット上に動力機構が搭載できず、何らかの形で連結する別の動力発生ユニットから駆動に必要な動力を得る必要がある。
【課題を解決するための手段】
【０００４】
本発明は上記課題を解決するためになされたもので、機構上の最少構成を有しながら、後方動力ユニットから先方駆動ユニットへ駆動力を伝達するとともに、目視することのできない先行する第１ユニットの現在の位置姿勢を検出することを可能とし、しかもその第１ユニットの現在の姿勢から所望の角度の姿勢に変更することができる能動連結機構及びその位置姿勢の検出方法及びその制御方法を提供することを目的とするものである。
【０００５】
本発明の能動連結機構は、先行する第１ユニットと後行する第２ユニットとを連結し、第２ユニットから第１ユニットへ駆動力を伝達する能動連結機構であって、この能動連結機構は、一対の関節駆動軸と推進駆動軸からなり、前記各軸の中央部が三次元ジョイントにより接合され、前記関節駆動軸は、両端部に互いに逆向きのネジが形成されて回動可能に軸支され、回動により三次元ジョイントで接合された関節駆動軸の各軸要素が各々のユニットから対称に出退して、各ユニット間の関節駆動軸の長さを可変とし、推進駆動軸は、両端が各ユニットに回転可能に軸支されると共に、各ユニット間の長さは固定され、中央部の三次元ジョイントを中心として第１ユニットを操作することを特徴とする。
【０００６】
また、本発明の能動連結機構は、一対の関節駆動軸と推進駆動軸が逆２等辺三角形に配置されたことを特徴とする。
本発明の上記構成により、一対の関節駆動軸の寸法を検出することにより、推進駆動軸の三次元ジョイントの関節角を求め、さらに第１ユニットの先端位置姿勢を求めることができる。
【０００７】
さらに、本発明の上記構成により、先端位置姿勢の実現に必要な推進駆動軸の関節角を求め、この推進駆動軸の三次元ジョイントの関節角から関節駆動軸の寸法を求め、この関節駆動軸寸法に一対の関節駆動軸を適合させることにより、第１ユニットを所望の先端位置姿勢にすることができる。
【０００８】
さらに、本発明の上記構成により、第１ユニット先端位置姿勢の検出と制御が容易にできるともに、前記各軸の中央部が前記三次元ジョイントにより結合されていても、本発明の能動連結機構で連結される第１ユニットと第２ユニットとの間は各駆動軸線を中心に相捻れるような運動の自由度が発生することはなく、各駆動軸により第２ユニットから第１ユニットへ駆動力を伝達することができる。
【０００９】
さらに、本発明の能動連結機構は、動作途中のどの状態においても、特異姿勢となることがない。即ち、一対の関節駆動軸と推進駆動軸全体での自由度が縮退して第１ユニットの動きが取れなくなることはない。さらに、一対の関節駆動軸と推進駆動軸全体で所望外の自由度が発生して第１ユニットを所定の姿勢で固定できなくなることはない。
また、本発明の能動連結機構の位置姿勢の検出方法は、中央部が三次元ジョイントにより接合された一対の関節駆動軸と推進駆動軸からなり、先行する第１ユニットと後行する第２ユニットとを連結し、第２ユニットから第１ユニットへ駆動力を伝達する能動連結機構の位置姿勢の検出方法において、前記一対の関節駆動軸の寸法を検出し、この検出した関節駆動軸の寸法から、推進駆動軸の三次元ジョイントの関節角を求めた後、第１ユニットの先端位置姿勢を求めることを特徴とする。
【００１０】
また、本発明の能動連結機構の制御方法は、中央部が三次元ジョイントにより接合された一対の関節駆動軸と推進駆動軸からなり、先行する第１ユニットと後行する第２ユニットとを連結し、第２ユニットから第１ユニットへ駆動力を伝達する能動連結機構の制御方法において、所望の第１ユニットの先端位置姿勢を決定し、この先端位置姿勢の実現に必要な推進駆動軸の関節角を求め、この推進駆動軸の三次元ジョイントの関節角から関節駆動軸の変位寸法量を求め、この関節駆動軸の変位寸法量に一対の関節駆動軸の寸法を適合させる操作を行うことを特徴とする。
【発明の効果】
【００１１】
本発明の上記構成により、先行する第１ユニットで必要な駆動力をその後方に連結される第２ユニットから伝達することができる。さらに、目視できない先行する第１ユニットの姿勢を、一対の関節駆動軸の寸法から正確に割り出すことができる。さらに、目視できない第１ユニットを所望の姿勢に制御することができる。
【発明を実施するための最良の形態】
【００１２】
以下に、本発明を実施するための最良の形態について、図面を用いて説明する。
図１は、本発明の動力伝達機能を有する能動連結機構の概略斜視図である。この能動連結機構は、先行する第１ユニット１と、この第１ユニット１の後方に接合する第２ユニット２との間に配置されて、第２ユニット２の後端に接続されたケーブル３を介して制御装置を通して第１ユニット１の位置姿勢を検出したり、制御するものである。
【００１３】
図１には図示していないが、第１ユニット１には、例えばＣＣＤカメラや温度や湿度等を測定するセンサーが搭載されている。ただし、この第１ユニット１には、電源（バッテリ）や自走するための駆動装置（具体的にはモータを含めた駆動機構）は搭載されていない。駆動力は第２ユニット２から伝達される。また、電力はケーブル３を通して送られている。第１ユニット１の外壁面には無限軌道（クローラ）４が取り付けられており、この無限軌道４により第１ユニット１が移動し、第２ユニットがそれに牽引されて移動できるように構成されている。なお、この図１では原理だけを説明するため、詳細な構成は省略してある。なお、無限軌道４は後述する推進駆動軸５からの駆動力に基づいて駆動するものである。
【００１４】
第２ユニット２には、関節駆動装置６（以下、関節駆動モータという）、推進駆動装置７（以下、推進駆動モータという）、関節駆動モータ６又は関節駆動軸８、９の回転数をカウントするエンコーダ１０（ＥＮ）等が備えられており、その他、第１ユニット１を支援するための装置が搭載されている。関節駆動モータ６は後述するように左右の関節駆動軸８、９を回転して第１ユニット１の姿勢を変えるための駆動力を発生させるためのモータである。推進駆動モータ７は第１ユニット１の無限軌道４に回転駆動力を与えるためのモータである。なお、ケーブル３は制御装置とユニット１、２間の信号のやり取りを行うと共に、第１ユニット１と第２ユニット２の電力を送電するためのケーブルである。
【００１５】
次に、能動連結機構の構成について説明する。能動連結機構は左右一対の関節駆動軸８、９と、１本の推進駆動軸５から構成されている。関節駆動軸８、９は第１ユニット１の姿勢を制御するものであり、推進駆動軸５は、前記のように第１ユニット１に回転駆動力を伝達するものである。この２本の関節駆動軸８、９と１本の推進駆動軸５との配置は、関節駆動軸８、９が上部に平行に所定間隔を開けて配置されており、この２本の関節駆動軸８、９に対して逆２等辺三角形となる位置に推進駆動軸５が配置されている。
関節駆動軸８、９は、図２に示すように、２本の軸要素８ａ、８ｂ及び軸要素９ａ，９ｂが中央部で三次元ジョイント１１、１２（ユニバーサルジョイント、自在継手、万能継手等で呼ばれる三次元方向に曲がる継ぎ手を言う。またスプリングのようなものであってもよい。以下、図示するユニバーサルジョイントの用語を用いて説明する）により連結され、このユニバーサルジョイント１１、１２を挟んでそれぞれの軸要素８ａ、８ｂ、９ａ、９ｂを上下、左右、斜め方向に曲げることができる。関節駆動軸８、９の両端部には、それぞれ逆方向の右ネジ１５と左ネジ１４が所定の長さだけ同ピッチで雄ネジとして刻まれている。さらに、この右ネジ１５と左ネジ１４に、それぞれ雌ネジ部材１６（ナット）が螺合している。これらの雌ネジ部材１６はそれぞれの第１ユニット１、第２ユニット２の基台１７、１８に固定されて移動は不可能である。これに対して、関節駆動軸８、９は、雌ネジ部材１６に螺合した右ネジ１５と左ネジ１４によって、ユニバーサルジョイント１１、１２を挟んで連結している２本の軸要素８ａ、８ｂ、９ａ、９ｂが、関節駆動軸８、９の正逆方向の回転により、対称（ユニバーサルジョイントを挟んで逆方向）に移動する。このため、ユニバーサルジョイント１１、１２を挟んで接合した各２本の軸要素８ａ、８ｂ、９ａ、９ｂが同じ長さだけ第１ユニット１と第２ユニッオ２から出入りして両雌ネジ部材１６、１６間の関節駆動軸８、９の全体の長さが伸縮する。このユニバーサルジョイント１１、１２をはさんでそれぞれの軸要素８ａ、８ｂ、９ａ、９ｂがそれぞれのユニット１，２から出入りすることを「関節対称駆動」という。これについてはさらに詳細に説明する。
【００１６】
第２ユニット２側の関節駆動軸８、９の一端には、関節駆動モータ６が取り付けられている。この関節駆動モータ６は正逆に回転することが可能でなければならず、しかも前記関節駆動軸８、９を回転して第１ユニット１の姿勢を操作するに十分な回転力（トルク）を備えたモータを選定しなければならない。この関節駆動モータ６は、関節駆動軸８、９の回転に伴って同じように回転してはならないが、この関節駆動モータ６の回転軸の軸芯方向にスライド可能な構造（図示せず）でなければならない。
【００１７】
関節駆動軸８、９に近接して、この駆動軸８、９の回転数をカウントするエンコーダ１０(EN)が備え付けられている。図２のエンコーダは関節駆動軸８，９のマーカをエンコーダ１０の検知器がカウントして関節駆動軸８、９の回転数を検知するようにしているが、このエンコーダ１０は他の構成であってもよく、例えば、関節駆動モータ６の回転数を検知するようなものであってもよい。このエンコーダ１０による関節駆動軸８、９の回転数の検出により、ユニット間（雌ネジ部材１６間）の関節駆動軸８、９の全長が判明する。
【００１８】
推進駆動軸５は、第１ユニット１の無限軌道４を駆動する駆動機構（図示せず）に推進駆動モータ７の回転力を伝達するための駆動軸である。この推進駆動軸５は２本の軸要素５ａ、５ｂが中央部でユニバーサルジョイント１３により連結され、このユニバーサルジョイント１３をはさんでそれぞれ上下、左右、斜め方向に曲げることができる。しかし、この推進駆動軸５は前記の関節駆動軸８、９と異なり両端部に左右の逆ネジは形成されておらず、回転は可能であるが、正逆方向の回動に伴って軸支された位置が変更することがない。即ち、前記関節駆動軸８、９のように正逆方向の回転により、第１ユニット１と第２ユニット２から軸要素５ａ、５ｂが対称に出入りすることがなく、回動は可能であるが軸芯方向に移動することなく、軸支された状態を維持する。従って、この推進駆動軸５の寸法により第１ユニット１と第２ユニット２との間隔が定まり、しかもユニバーサルジョイント１３がヒンジの役割を果たし、ユニバーサルジョイント１３を中心にして第２ユニットに対して第１ユニット１が回動することができる。
【００１９】
第２ユニット２側の前記推進駆動軸５の端部に推進駆動モータ７が接続している。推進駆動モータ７は前記のように第１ユニット１の無限軌道４を駆動するに十分な回転力（トルク）を出力できるものを選択しなければならない。この推進駆動モータ７は第２ユニット２の基台１８に固定されており、関節駆動モータ６のように、推進駆動モータ７の回転軸の軸芯方向にスライドしながら回転することはない。従って、前記のように推進駆動軸５も軸芯方向に動くことはない。第１ユニット１側の推進駆動軸５の端部は軸受（図示せず）に軸支されている。この軸支の構成も推進駆動軸５が、軸芯方向に移動することが出来ないように、固定されている。
【００２０】
次に、前記構成の作動原理について説明する。
図３、４は本件発明の能動連結機構の作動原理を説明するための説明図である。この図面は第１ユニットを円筒状に簡略化して描いている。これらの図において、前記の説明と同様の部材は同一の符号を付してある。また、第１ユニットが回動する基準軸は図１に示す、Ｙ軸、Ｘ軸、Ｚ軸として説明する。
【００２１】
本発明の能動連結機構は、第１ユニット１が瓦礫等の中を探査していて、その姿勢等が不明であっても、「順運動学関係」から、関節駆動軸８、９の変位寸法量ｄｌ、ｄｒ（数式上、関節駆動軸変数と言うことがある）及びそれから導きだされる関節角θ１、θ２から第１ユニット１の位置、姿勢を解析することが可能となる。また、「逆運動学関係」から、第１ユニット１の目標位置の条件を設定すると、それを満たす関節角θ１、θ２及び関節駆動軸８、９の変位寸法量ｄｌ、ｄｒを求めて第１ユニット１を所望の姿勢となるように操作することができる。以下、これらの点について説明する。
Ｉ）第１ユニットの先端位置姿勢を求める。（順運動学関係） この順運動学関係は、前記のように第１ユニット１の先端位置姿勢が不明であっても、関節駆動軸８、９の変位寸法量ｄｌ、ｄｒ（左右関節駆動軸変数）から、関節角θ１、θ２を求め、さらに、第１ユニットのユニット先端位置姿勢を求めるものである。ユニット先端位置姿勢とは、第１ユニットの特定の位置（例えば、ＣＣＤカメラのレンズの位置）の座標軸の距離及び角度から第１ユニットの位置姿勢を表すものである。
【００２２】
前記のｄｌは、左側（left）の関節駆動軸８の変位寸法量、ｄｒは、右側（right）の関節駆動軸９の変位寸法量を示している。さらに変位寸法量ｄｌ、ｄｒを図１、図２を用いて具体的に説明する。変位寸法量ｄｌ、ｄｒは、折れ曲がった状態を含めて、両雌ネジ部材１６，１６間の関節駆動軸８、９の軸要素８ａ，８ｂ，９ａ，９ｂの長さ（もしくは下記基準姿勢における基準寸法に対する変化量）であるが、第１ユニット１と、第２ユニット２間の関節駆動軸８，９の軸要素８ａ，８ｂ，９ａ，９ｂの長さ（もしくは下記基準姿勢における基準寸法に対する変化量）と言い表すこともできる。この変位寸法量ｄｌ、ｄｒは、前記の説明のとおり、関節駆動モータ６、６により、関節駆動軸８、９を正逆方向に回転することにより、両ユニット間の全長（以下、単に寸法という）は変化する。前記のように、図２の雌ネジ部材１６，１６に螺合している関節駆動軸８，９には右ネジ１５と左ネジ１４が刻まれているので、関節駆動モータ６、６の回転により、ユニバーサルジョイント１１，１２を挟んで連結されたそれぞれの軸要素８ａ，８ｂ，９ａ，９ｂは、同寸法だけ対称（軸要素がそれぞれ逆方向）に両ユニット１，２から出入りする。第１ユニット１と第２ユニット２が、図３において、第１ユニットを点線で示すように一直線状の姿勢（以下、基準姿勢という）の場合の両ユニット１，２間の関節駆動軸８，９の寸法を基準寸法とすると、関節駆動モータ６，６の例えば正回転により軸要素８ａ，８ｂ，９ａ，９ｂがそれぞれのユニット１，２に退いていく（中に入る状態）とすると、両ユニット１，２間の関節駆動軸８，９の寸法は、前記基準寸法より短くなる。逆に、関節駆動モータ６，６を逆回転すると、軸要素８ａ，８ｂ，９ａ，９ｂはそれぞれのユニット１，２から同じ寸法だけ出てくる（中から出てくる状態）ので、両ユニット１，２間の関節駆動軸８，９の寸法は前記の基準寸法より長くなる。この両ユニット１，２間の関節駆動軸８，９の軸要素８ａ，８ｂ，９ａ，９ｂの伸縮する寸法を変位寸法量ｄｌ、ｄｒと言う。この変位寸法量ｄｌ、ｄｒは、第１ユニットの先端位置姿勢を求める際に、数式を解析する場合は左右の関節駆動軸の左右関節駆動軸変数と言う。
この関節駆動軸８，９の左右関節駆動軸変数ｄｌ、ｄｒから、後述する関数関係を解析することにより、関節角θ１、θ２を求めることができる。関節角θ１は、図３に示すように、第１ユニット１が、上下方向に回転した場合の回転角である。より正確には、推進駆動軸５のユニバーサルジョイント１３からＹ軸方向に垂直に伸びる軸線２０（仮想Ｙ軸基準線）が、変位寸法量ｄｌ、ｄｒの変化に伴って、前記ユニバーサルジョイント１３の位置におけるＺ軸線を中心として回動した場合の回転角である。図３は、第１ユニット１がユニバーサルジョイント１３のＺ軸線を中心にθ１だけ回転して上方に持上げられた状態を示す正面図である。関節角θ２は、変位寸法量ｄｌ、ｄｒの変化に伴って、第１ユニット１が、仮想Ｙ軸基準線２０を中心として、左右方向に回動した場合の回転角である。図４の平面図は、図３の正面図の状態から第１ユニット１がさらに仮想Ｙ軸基準線２０を中心にしてθ２だけ左方向に回転した状態を示している。
【００２３】
次に、関節駆動軸の変位寸法量ｄｌ、ｄｒと、関節角θ１、θ２との関係について説明する。
１）関節駆動軸要素８ａ，８ｂの変位寸法量ｄｌと、同９ａ，９ｂの変位寸法量ｄｒとが同じだけ基準寸法より短い状態。
【００２４】
第１ユニット１が、基準姿勢の状態からユニバーサルジョイント１３のＺ軸線を中心に上方に曲がる。変位寸法量ｄｌ、ｄｒの変化量が大きくなると、関節角θ１は大きくなり、変位寸法量ｄｌ、ｄｒの変化量が小さくなると、関節角θ１は小さくなる。
２）関節駆動軸要素８ａ，８ｂの変位寸法量ｄｌと、同９ａ，９ｂが変位寸法量ｄｒとが同じだけ基準寸法より長い状態。
【００２５】
第１ユニット１が、基準姿勢の状態からユニバーサルジョイント１３のＺ軸を中心に下方に曲がる。変位寸法量ｄｌ、ｄｒの変化量が大きくなると、関節角−θ１の絶対値は大きくなり、変位寸法量ｄｌ、ｄｒの変化量が小さくなると、関節角−θ１の絶対値は小さくなる。
３）基準寸法より、左側関節駆動軸要素８a,８ｂの変位寸法量ｄｌが短く、右側関節駆動軸要素９ａ，９ｂの変位寸法量ｄｒが長い状態。
【００２６】
第１ユニット１が、基準姿勢の状態から仮想Ｙ軸基準線２０を中心に左に曲がる。変位寸法量ｄｌ、ｄｒの変化量が大きくなると関節角θ２は大きくなり、変位寸法量ｄｌ、ｄｒの変化量が小さくなると、関節角θ２は小さくなる。
４）基準寸法より、左側関節駆動軸要素８ａ，８ｂの変位寸法量ｄｌが長く、右側関節駆動軸要素９ａ，９ｂの変位寸法量ｄｒが短い状態。
【００２７】
第１ユニット１が、基準姿勢の状態から仮想Ｙ軸基準線２０を中心に右に曲がる。変位寸法量ｄｌ、ｄｒの変化量が小さくなると関節角−θ２の絶対値は小さくなり、変位寸法量ｄｌ、ｄｒの変化量が大きくなると、関節角−θ２の絶対値は大きくなる。
【００２８】
以上の１）２）３）４）の基本姿態をさらに組み合わせた、１）＋３）、１）＋４）、２）＋３）、２）＋４）の姿勢ができる。この姿勢は第１ユニット１がＺ軸線を中心に上下方向に曲がった状態から、さらに、左右の何れかの方向に回転して捻れた状態の組み合わせである。
【００２９】
上記の変位寸法量ｄｌ、ｄｒと関節角θ１、θ２とは幾何学的関係から次のような関数関係となる。
(ｄｌ、ｄｒ)＝ｇ（θ１、θ２） 式Ｉ
この式Ｉの逆関数は次の式IIのようになり、ｄｌ、ｄｒより関節角θ１、θ２を求める際に用いる順運動学関係である。
【００３０】
(θ１、θ２)＝ｇ-１(ｄｌ、ｄｒ) 式II
上記の式Ｉ、IIのｇは関数を表し、そのパラメータ値は、能動連結機構の種々の寸法等の要素によって定まる。例えば、推進駆動軸５と関節駆動軸８，９の寸法及び関節駆動軸８，９と推進駆動軸５との距離関係等から導き出されるものである。（ｄｌ、ｄｒ）は前記のように関節駆動モータ６の回転数をエンコーダ１０によりカウントすることにより判明する。左右関節駆動軸変数（ｄｌ、ｄｒ）よりｇの逆関数（式II）から関節角θ１、θ２を求めることができる。
【００３１】
また、能動連結機構を表す数学モデルとして関節座標系（ｘi,ｙi,ｚi）と関節動作を表す関節角θ１、θ２とを適正に設定することにより、基準座標系（ｘR,ｙR,ｚR）やユニット先端座標系（ｘE,ｙE,ｚE）を含めた座標系間の同時変換関係を表す行列
ｉ-１Ｔi＝ｉ-１Ｔi(θi)
の各要素を用いることにより、次のようなθ１、θ２から第１ユニット先端位置姿勢までの順運動学関係を求めることができる。
【００３２】
（ｘE,ｙE,ｚE,θE,φE,ψE）＝ｆ(θ１、θ２) 式III
上記式IIIの（ｘE,ｙE,ｚE）は、第１ユニット１の先端位置(Eed)における先端座標系の基準座標系に対する距離を示し、（θE,φE,ψE）は、先端座標系の基準座標系に対する角度を示している。この距離（ｘE,ｙE,ｚE）と角度（θE,φE,ψE）により第１ユニットの先端位置姿勢を表している。
【００３３】
また、式IIIのｆは関数（function）を示し、ユニット１、２や能動連結機構の寸法等によって定まるものである。
以上のような式Ｉ、式II、式IIIを用いて、左右関節駆動軸変数（関節駆動軸の変位寸法量）ｄｌ、ｄｒから、現在の第１ユニットの先端位置姿勢を求めることができる。
【００３４】
次に、第１ユニット１の現在の先端位置姿勢をパーソナルコンピュータにより求める場合を図５のフローチャートと、図７のブロック図を用いて説明する。
図７のＩ/Ｆ，ＣＰＵ，ＲＡＭ，ＲＯＭ，ＬＣＤ（液晶ディスプレイ）等はパーソナルコンピュータを示しているが、制御用の特別なマイクロコンピュータを用いたものであってもよい。図７において、６、６は関節駆動モータ、７は推進駆動モータ、１０，１０はエンコーダ（ＥＮ）を示している。
【００３５】
ステップ１；第１ユニット１の先端位置姿勢のｎ周期目の検出のスタートを示している。キーボードからの指令により、ＲＯＭ内に格納されているプログラムにより先端位置姿勢の検出を開始する。
【００３６】
ステップ２；左側の関節駆動軸８に設けられているエンコーダ１０(L)から左側関節駆動軸８の変位寸法量（ｄｌ）を検出する。即ち、左側のエンコーダ１０（L）の正逆方向の回転数から関節駆動軸８の現在の寸法を割り出し変位寸法量（ｄｌ）を検出する。
【００３７】
ステップ３；右側の関節駆動軸９に設けられているエンコーダ１０(R)から右側関節駆動軸９の変位寸法量（ｄｒ）を検出する。即ち、右側のエンコーダ１０（R）の正逆方向の回転数から関節駆動軸９の現在の寸法を割り出し変位寸法量（ｄｒ）を検出する。
【００３８】
ステップ４、５；前記ステップ２で検出した変位寸法量（ｄｌ）と、前記ステップ３で検出した変位寸法量（ｄｒ）とからＺ軸回りの関節角θ１と、仮想Ｙ軸基準線回りの関節角θ２とを算出する。この関節角θ１θ２を算出する式IIの関数関係は前記の通りである。
【００３９】
ステップ６；前記ステップ４、５で求めた関節角θ１θ２から第１ユニット１の先端位置姿勢（ｘE,ｙE,ｚE,θE,φE,ψE）を算出する。この第１ユニット１の先端位置姿勢は式IIIから求めることができることは、前記の通りである。
【００４０】
ステップ７；前記のステップ６で求めた結果をＬＣＤに表示する。第１ユニット１の先端位置姿勢（ｘE,ｙE,ｚE,θE,φE,ψE）を出力して、第１ユニット１の先端位置姿勢を画像としてＬＣＤに表示する。
【００４１】
ステップ８；ステップ７により求めた第１ユニット１の先端位置姿勢からｎ＋１周期目の検出に移行する。ｎ＋１周期目以降も同様にして刻々変化する第１ユニット１の先端位置姿勢を算出し、ＬＣＤに表示することができる。
II）第１ユニット先端目標位置実現のための変位寸法量を求める。（逆運動学関係）
逆運動学関係は、第１ユニット１の先端位置姿勢から関節駆動軸８，９の変位寸法量ｄｌ、ｄｒへの関係を与えるものであり、操縦者が所望する第１ユニット１の先端位置姿勢を実現するために必要な関節駆動軸８，９の変位寸法量ｄｌ、ｄｒを求めるのに用いる。即ち、第１ユニット１を目標とする位置姿勢にするために左右の関節駆動軸８，９の寸法（ｄｌ、ｄｒ）をどれだけにすればよいかを解析するものである。そのためには、第１ユニットの先端位置姿勢の実現に必要な回転角θｄ１、θｄ２を求め、さらに必要な関節駆動軸８，９の変位寸法量（左右関節駆動軸変数）ｄｄｌ、ｄｄｒを求める逆運動学関係を解析的に解くことにより、第１ユニット１の先端目標位置姿勢を実現する。
【００４２】
前記の順運動学関係の式IIIの逆関係である逆運動学関係は次の通り表されるとする。
(θ１、θ２)＝ｆ-１(ｘE,ｙE,ｚE,θE,φE,ψE) 式Ｖ
いま、第１ユニット１の先端目標位置姿勢を次のように与えられるとする。
【００４３】
（ｘE,ｙE,ｚE,θE,φE,ψE）ｄ 式VI
この式VIに逆運動学関係式Ｖを当てはめると、目標回転角θｄ１、θｄ２が求められる。
【００４４】
(θｄ１、θｄ２)＝ｆ-１（ｘE,ｙE,ｚE,θE,φE,ψE）ｄ
さらに、回転角θｄ１、θｄ２から関数式Ｉを用いて左右関節駆動軸変数（関節駆動軸の変位寸法量）ｄｄｌ、ｄｄrを求めることができる。
【００４５】
(ｄｄl、ｄｄr)＝ｇ（θｄ1、θｄ2）
このようにして求めた関節駆動軸の変位寸法量ｄｄl、ｄｄrに基づいて関節駆動モータ６を回動して回転駆動軸８，９の両ユニット１，２間の寸法をこの変位寸法量ｄｄl、ｄｄrに適合するように制御すれば、第１ユニット１の先端位置姿勢を実現することができる。
【００４６】
次に、前記の説明のうち、第１ユニットの先端位置姿勢を実現するための制御方法について、図６のフローチャートと、図７のブロック図を用いて説明する。図７のＩ/Ｆ，ＣＰＵ，ＲＡＭ，ＲＯＭ，ＬＣＤ等はパーソナルコンピュータを示しているが、制御用の特別なコントローラであってもよい。
【００４７】
先ず、前記のように、逆運動学関係により第１ユニット１の先端位置姿勢（ｘE,ｙE,ｚE,θE,φE,ψE）ｄを実現するために必要な第１ユニット１の回転角θｄ１、θｄ２を導き出し、ユニバーサルジョイント１３のＺ軸線を中心軸として第１ユニット１が上下に回動する回転角θｄ１と、仮想Ｙ軸基準線２０を中心軸として第１ユニット１が左右に回動する回転角θｄ２に分解する。分解した回転角θｄ１のＺ軸線を中心軸とする動作指令と、回転角θｄ２の仮想Ｙ軸基準線２０を中心軸とする動作指令を出す。（ステップ１）
次に、（ステップ１）で分解した回転角θｄ１から左側関節駆動軸８の関節駆動変位寸法量ｄｄl 1、と右側関節駆動軸９の関節駆動変位寸法量ｄｄr 1とに分解する。（ステップ２）
さらに、（ステップ１）で分解した回転角θｄ２から左側関節駆動軸８の関節駆動変位寸法量ｄｄl 2、と右側関節駆動軸９の関節駆動変位寸法量ｄｄr 2とに分解する。（ステップ３）
次に、ステップ２とステップ３で分解した関節駆動変位寸法量ｄｄl 1、関節駆動変位寸法量ｄｄｒ1、関節駆動変位寸法量ｄｄl 2、関節駆動変位寸法量ｄｄｒ 2を用いて、ｄｄl 1とｄｄl 2とを足し合わせて左側関節駆動軸８の目標変位寸法量ｄｄlを得（ステップ４）、ｄｄｒ1 とｄｄｒ 2とを足し合わせて右側関節駆動軸９の目標変位寸法量ｄｄｒを得（ステップ５）、それらをサーボ制御により実現する。
【００４８】
図６の左端のシーケンスは、左側関節駆動軸８の目標変位寸法量ｄｄｌ を実現するためのサーボ制御をデジタルコンピュータを用いて離散的に実現する場合を示している。このサーボ制御（Ｓ.Ｃ.Ｉ）はステップ４によって得られる左側関節駆動軸８の目標変位寸法量ｄｄｌと、現在の左側の関節駆動軸８の変位寸法量ｄｌを比較する。左側関節駆動軸８の目標変位寸法量ｄｄｌと、現在の左側の関節駆動軸８の変位寸法量ｄｌとの差が０の場合、即ち左側関節駆動軸８の変位寸法量ｄｌを変える必要がない場合は現在のｄｌを保持し次ぎの制御周期に移る。
もし、左側関節駆動軸の目標変位寸法量ｄｄｌと、現在の左側の関節駆動軸８の変位寸法量ｄｌ間に誤差がある場合は、その誤差を修正する制御を行う。即ち、エンコーダ１０（ＥＮ）により関節駆動軸８の回転数をカウントして、得られる現在の変位寸法量ｄｌを、ステップ４で得られる左側関節駆動軸の目標変位寸法量ｄｄｌと比較する。そして、その差に対してゲイン（誤差の変換・増幅率）Ｃの定めに基づき増幅し、さらにドライバによりモータ駆動入力に増幅し、左側のアクチュエータ（関節駆動モータ６）に通電し、左側関節駆動軸の目標変位寸法量ｄｄｌと、現在の左側関節駆動軸の変位寸法量ｄｌとの間の差が０となる方向に回転する。このようなサーボ制御により、左側関節駆動軸の目標変位寸法量ｄｄｌと、現在の左側関節駆動軸の変位寸法量ｄｌの誤差を０にする。
シーケンス（Ｓ.Ｃ.Ｉ）と同時に右隣りの第２の制御シーケンス（Ｓ.Ｃ.II）を同様に実施して、ステップ５で得られる右側関節駆動軸の目標変位寸法量ｄｄｒと、現在の右側の関節駆動軸の変位寸法量ｄｒを比較して、誤差がある場合はアクチュエータにより修正して誤差をなくすよう制御を行う。
【産業上の利用可能性】
【００４９】
本発明の応用例の一例として震災や災害時に建造物等の瓦礫の中を探査するための探査用装置に応用することができる。
【図面の簡単な説明】
【００５０】
【図１】本発明の能動連結機構の斜視図である。
【図２】本発明の能動連結機構の部分構成図である。
【図３】本発明の能動連結機構の作動原理を説明するための説明図である。
【図４】本発明の能動連結機構の作動原理を説明するための説明図である。
【図５】本発明の能動連結機構の位置姿勢の検出方法を示すフローチャートである。
【図６】本発明の能動連結機構の制御方法を示すフローチャートである。
【図７】本発明の能動連結機構の検出部・制御部等のブロック図である。
【符号の説明】
【００５１】
１ 第１ユニット
２ 第２ユニット
３ ケーブル
４ 無限軌道
５ 推進駆動軸
６ 関節駆動装置
７ 推進駆動装置
８ 関節駆動軸
８ａ 軸要素
８ｂ 軸要素
９ 関節駆動軸
９ａ 軸要素
９ｂ 軸要素
１０ エンコーダ
１１ ユニバーサルジョイント
１２ ユニバーサルジョイント
１３ ユニバーサルジョイント
１４ 左ネジ
１５ 右ネジ
１６ 雌ネジ部材
１７ 基台
１８ 基台
２０ 仮想Ｙ軸基準線
【技術分野】
【０００１】
本発明は、先進する第１ユニットに、連結する後方の第２ユニットから駆動力を伝達する能動連結機構及びその位置姿勢の状態を検出すると共に、第１ユニットを所望の姿勢に制御するその制御方法に関し、本発明は、種々の装置に応用できるが、その応用例の一例として震災や災害時に建造物等の瓦礫の中を探査するための探査用装置や原子力発電所等の危険な領域での監視システム等に応用することのできる能動連結機構及びその位置姿勢の検出方法及びその制御方法に関する。
【背景技術】
【０００２】
前記のように、震災や災害時に建造物等の瓦礫の中を探査する際、生存者の救出のために、種々の探査用装置が開発されている。その一例として特開２００４−３５０８８９号公報に簡易型探査用レスキュー装置が掲載されている。この特許文献記載のレスキュー装置は先端部に車輪を有する探査部を備え、駆動ハンドルから駆動用ワイヤを介して車輪に駆動力を伝達している。さらに、２本の操舵用ワイヤがケーブル部内に配置され、その先端は前記探査部の背面側の左右両側にそれぞれ１本ずつ接続され、他端には操舵用ハンドルが接続されている。操縦者はケーブル部の後端部を把持しながら、操舵用ハンドルを牽引することにより探査部の方向を左右に操縦している。
【特許文献１】特開２００４−３５０８８９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、前記従来の探査用レスキュー装置の探査部は瓦礫の中に完全に入り込んで隠れた状態にあることもあり、先端の探査部の姿勢が左右方向、上下方向にどれだけが傾いているのかその姿勢を正確につかむことができず、カメラに写し出された映像もどのような角度で写し出されているのか判然としない。しかも、現在の姿勢から所望の角度だけ探査部の角度を変えようとしても探査部が目視できないため、不可能であった。かりに、探査部の姿勢を検知するとすれば、探査部の角度を検出する角度検出センサーを探査部の外部等に装備する必要がある。さらに、２本の操舵用ワイヤを牽引しても探査部は左右方向の操縦しかできず、探査部を上方向に持上げて障害物を乗り越えるようにするためには、さらに操舵用ワイヤを１本増やさなければならない。また、狭隘な瓦礫等の中に入り込む場合、探査部の小型化の必要性、それによる探査部の搭載スペースの制約によって、探査部と同一のユニット上に動力機構が搭載できず、何らかの形で連結する別の動力発生ユニットから駆動に必要な動力を得る必要がある。
【課題を解決するための手段】
【０００４】
本発明は上記課題を解決するためになされたもので、機構上の最少構成を有しながら、後方動力ユニットから先方駆動ユニットへ駆動力を伝達するとともに、目視することのできない先行する第１ユニットの現在の位置姿勢を検出することを可能とし、しかもその第１ユニットの現在の姿勢から所望の角度の姿勢に変更することができる能動連結機構及びその位置姿勢の検出方法及びその制御方法を提供することを目的とするものである。
【０００５】
本発明の能動連結機構は、先行する第１ユニットと後行する第２ユニットとを連結し、第２ユニットから第１ユニットへ駆動力を伝達する能動連結機構であって、この能動連結機構は、一対の関節駆動軸と推進駆動軸からなり、前記各軸の中央部が三次元ジョイントにより接合され、前記関節駆動軸は、両端部に互いに逆向きのネジが形成されて回動可能に軸支され、回動により三次元ジョイントで接合された関節駆動軸の各軸要素が各々のユニットから対称に出退して、各ユニット間の関節駆動軸の長さを可変とし、推進駆動軸は、両端が各ユニットに回転可能に軸支されると共に、各ユニット間の長さは固定され、中央部の三次元ジョイントを中心として第１ユニットを操作することを特徴とする。
【０００６】
また、本発明の能動連結機構は、一対の関節駆動軸と推進駆動軸が逆２等辺三角形に配置されたことを特徴とする。
本発明の上記構成により、一対の関節駆動軸の寸法を検出することにより、推進駆動軸の三次元ジョイントの関節角を求め、さらに第１ユニットの先端位置姿勢を求めることができる。
【０００７】
さらに、本発明の上記構成により、先端位置姿勢の実現に必要な推進駆動軸の関節角を求め、この推進駆動軸の三次元ジョイントの関節角から関節駆動軸の寸法を求め、この関節駆動軸寸法に一対の関節駆動軸を適合させることにより、第１ユニットを所望の先端位置姿勢にすることができる。
【０００８】
さらに、本発明の上記構成により、第１ユニット先端位置姿勢の検出と制御が容易にできるともに、前記各軸の中央部が前記三次元ジョイントにより結合されていても、本発明の能動連結機構で連結される第１ユニットと第２ユニットとの間は各駆動軸線を中心に相捻れるような運動の自由度が発生することはなく、各駆動軸により第２ユニットから第１ユニットへ駆動力を伝達することができる。
【０００９】
さらに、本発明の能動連結機構は、動作途中のどの状態においても、特異姿勢となることがない。即ち、一対の関節駆動軸と推進駆動軸全体での自由度が縮退して第１ユニットの動きが取れなくなることはない。さらに、一対の関節駆動軸と推進駆動軸全体で所望外の自由度が発生して第１ユニットを所定の姿勢で固定できなくなることはない。
また、本発明の能動連結機構の位置姿勢の検出方法は、中央部が三次元ジョイントにより接合された一対の関節駆動軸と推進駆動軸からなり、先行する第１ユニットと後行する第２ユニットとを連結し、第２ユニットから第１ユニットへ駆動力を伝達する能動連結機構の位置姿勢の検出方法において、前記一対の関節駆動軸の寸法を検出し、この検出した関節駆動軸の寸法から、推進駆動軸の三次元ジョイントの関節角を求めた後、第１ユニットの先端位置姿勢を求めることを特徴とする。
【００１０】
また、本発明の能動連結機構の制御方法は、中央部が三次元ジョイントにより接合された一対の関節駆動軸と推進駆動軸からなり、先行する第１ユニットと後行する第２ユニットとを連結し、第２ユニットから第１ユニットへ駆動力を伝達する能動連結機構の制御方法において、所望の第１ユニットの先端位置姿勢を決定し、この先端位置姿勢の実現に必要な推進駆動軸の関節角を求め、この推進駆動軸の三次元ジョイントの関節角から関節駆動軸の変位寸法量を求め、この関節駆動軸の変位寸法量に一対の関節駆動軸の寸法を適合させる操作を行うことを特徴とする。
【発明の効果】
【００１１】
本発明の上記構成により、先行する第１ユニットで必要な駆動力をその後方に連結される第２ユニットから伝達することができる。さらに、目視できない先行する第１ユニットの姿勢を、一対の関節駆動軸の寸法から正確に割り出すことができる。さらに、目視できない第１ユニットを所望の姿勢に制御することができる。
【発明を実施するための最良の形態】
【００１２】
以下に、本発明を実施するための最良の形態について、図面を用いて説明する。
図１は、本発明の動力伝達機能を有する能動連結機構の概略斜視図である。この能動連結機構は、先行する第１ユニット１と、この第１ユニット１の後方に接合する第２ユニット２との間に配置されて、第２ユニット２の後端に接続されたケーブル３を介して制御装置を通して第１ユニット１の位置姿勢を検出したり、制御するものである。
【００１３】
図１には図示していないが、第１ユニット１には、例えばＣＣＤカメラや温度や湿度等を測定するセンサーが搭載されている。ただし、この第１ユニット１には、電源（バッテリ）や自走するための駆動装置（具体的にはモータを含めた駆動機構）は搭載されていない。駆動力は第２ユニット２から伝達される。また、電力はケーブル３を通して送られている。第１ユニット１の外壁面には無限軌道（クローラ）４が取り付けられており、この無限軌道４により第１ユニット１が移動し、第２ユニットがそれに牽引されて移動できるように構成されている。なお、この図１では原理だけを説明するため、詳細な構成は省略してある。なお、無限軌道４は後述する推進駆動軸５からの駆動力に基づいて駆動するものである。
【００１４】
第２ユニット２には、関節駆動装置６（以下、関節駆動モータという）、推進駆動装置７（以下、推進駆動モータという）、関節駆動モータ６又は関節駆動軸８、９の回転数をカウントするエンコーダ１０（ＥＮ）等が備えられており、その他、第１ユニット１を支援するための装置が搭載されている。関節駆動モータ６は後述するように左右の関節駆動軸８、９を回転して第１ユニット１の姿勢を変えるための駆動力を発生させるためのモータである。推進駆動モータ７は第１ユニット１の無限軌道４に回転駆動力を与えるためのモータである。なお、ケーブル３は制御装置とユニット１、２間の信号のやり取りを行うと共に、第１ユニット１と第２ユニット２の電力を送電するためのケーブルである。
【００１５】
次に、能動連結機構の構成について説明する。能動連結機構は左右一対の関節駆動軸８、９と、１本の推進駆動軸５から構成されている。関節駆動軸８、９は第１ユニット１の姿勢を制御するものであり、推進駆動軸５は、前記のように第１ユニット１に回転駆動力を伝達するものである。この２本の関節駆動軸８、９と１本の推進駆動軸５との配置は、関節駆動軸８、９が上部に平行に所定間隔を開けて配置されており、この２本の関節駆動軸８、９に対して逆２等辺三角形となる位置に推進駆動軸５が配置されている。
関節駆動軸８、９は、図２に示すように、２本の軸要素８ａ、８ｂ及び軸要素９ａ，９ｂが中央部で三次元ジョイント１１、１２（ユニバーサルジョイント、自在継手、万能継手等で呼ばれる三次元方向に曲がる継ぎ手を言う。またスプリングのようなものであってもよい。以下、図示するユニバーサルジョイントの用語を用いて説明する）により連結され、このユニバーサルジョイント１１、１２を挟んでそれぞれの軸要素８ａ、８ｂ、９ａ、９ｂを上下、左右、斜め方向に曲げることができる。関節駆動軸８、９の両端部には、それぞれ逆方向の右ネジ１５と左ネジ１４が所定の長さだけ同ピッチで雄ネジとして刻まれている。さらに、この右ネジ１５と左ネジ１４に、それぞれ雌ネジ部材１６（ナット）が螺合している。これらの雌ネジ部材１６はそれぞれの第１ユニット１、第２ユニット２の基台１７、１８に固定されて移動は不可能である。これに対して、関節駆動軸８、９は、雌ネジ部材１６に螺合した右ネジ１５と左ネジ１４によって、ユニバーサルジョイント１１、１２を挟んで連結している２本の軸要素８ａ、８ｂ、９ａ、９ｂが、関節駆動軸８、９の正逆方向の回転により、対称（ユニバーサルジョイントを挟んで逆方向）に移動する。このため、ユニバーサルジョイント１１、１２を挟んで接合した各２本の軸要素８ａ、８ｂ、９ａ、９ｂが同じ長さだけ第１ユニット１と第２ユニッオ２から出入りして両雌ネジ部材１６、１６間の関節駆動軸８、９の全体の長さが伸縮する。このユニバーサルジョイント１１、１２をはさんでそれぞれの軸要素８ａ、８ｂ、９ａ、９ｂがそれぞれのユニット１，２から出入りすることを「関節対称駆動」という。これについてはさらに詳細に説明する。
【００１６】
第２ユニット２側の関節駆動軸８、９の一端には、関節駆動モータ６が取り付けられている。この関節駆動モータ６は正逆に回転することが可能でなければならず、しかも前記関節駆動軸８、９を回転して第１ユニット１の姿勢を操作するに十分な回転力（トルク）を備えたモータを選定しなければならない。この関節駆動モータ６は、関節駆動軸８、９の回転に伴って同じように回転してはならないが、この関節駆動モータ６の回転軸の軸芯方向にスライド可能な構造（図示せず）でなければならない。
【００１７】
関節駆動軸８、９に近接して、この駆動軸８、９の回転数をカウントするエンコーダ１０(EN)が備え付けられている。図２のエンコーダは関節駆動軸８，９のマーカをエンコーダ１０の検知器がカウントして関節駆動軸８、９の回転数を検知するようにしているが、このエンコーダ１０は他の構成であってもよく、例えば、関節駆動モータ６の回転数を検知するようなものであってもよい。このエンコーダ１０による関節駆動軸８、９の回転数の検出により、ユニット間（雌ネジ部材１６間）の関節駆動軸８、９の全長が判明する。
【００１８】
推進駆動軸５は、第１ユニット１の無限軌道４を駆動する駆動機構（図示せず）に推進駆動モータ７の回転力を伝達するための駆動軸である。この推進駆動軸５は２本の軸要素５ａ、５ｂが中央部でユニバーサルジョイント１３により連結され、このユニバーサルジョイント１３をはさんでそれぞれ上下、左右、斜め方向に曲げることができる。しかし、この推進駆動軸５は前記の関節駆動軸８、９と異なり両端部に左右の逆ネジは形成されておらず、回転は可能であるが、正逆方向の回動に伴って軸支された位置が変更することがない。即ち、前記関節駆動軸８、９のように正逆方向の回転により、第１ユニット１と第２ユニット２から軸要素５ａ、５ｂが対称に出入りすることがなく、回動は可能であるが軸芯方向に移動することなく、軸支された状態を維持する。従って、この推進駆動軸５の寸法により第１ユニット１と第２ユニット２との間隔が定まり、しかもユニバーサルジョイント１３がヒンジの役割を果たし、ユニバーサルジョイント１３を中心にして第２ユニットに対して第１ユニット１が回動することができる。
【００１９】
第２ユニット２側の前記推進駆動軸５の端部に推進駆動モータ７が接続している。推進駆動モータ７は前記のように第１ユニット１の無限軌道４を駆動するに十分な回転力（トルク）を出力できるものを選択しなければならない。この推進駆動モータ７は第２ユニット２の基台１８に固定されており、関節駆動モータ６のように、推進駆動モータ７の回転軸の軸芯方向にスライドしながら回転することはない。従って、前記のように推進駆動軸５も軸芯方向に動くことはない。第１ユニット１側の推進駆動軸５の端部は軸受（図示せず）に軸支されている。この軸支の構成も推進駆動軸５が、軸芯方向に移動することが出来ないように、固定されている。
【００２０】
次に、前記構成の作動原理について説明する。
図３、４は本件発明の能動連結機構の作動原理を説明するための説明図である。この図面は第１ユニットを円筒状に簡略化して描いている。これらの図において、前記の説明と同様の部材は同一の符号を付してある。また、第１ユニットが回動する基準軸は図１に示す、Ｙ軸、Ｘ軸、Ｚ軸として説明する。
【００２１】
本発明の能動連結機構は、第１ユニット１が瓦礫等の中を探査していて、その姿勢等が不明であっても、「順運動学関係」から、関節駆動軸８、９の変位寸法量ｄｌ、ｄｒ（数式上、関節駆動軸変数と言うことがある）及びそれから導きだされる関節角θ１、θ２から第１ユニット１の位置、姿勢を解析することが可能となる。また、「逆運動学関係」から、第１ユニット１の目標位置の条件を設定すると、それを満たす関節角θ１、θ２及び関節駆動軸８、９の変位寸法量ｄｌ、ｄｒを求めて第１ユニット１を所望の姿勢となるように操作することができる。以下、これらの点について説明する。
Ｉ）第１ユニットの先端位置姿勢を求める。（順運動学関係） この順運動学関係は、前記のように第１ユニット１の先端位置姿勢が不明であっても、関節駆動軸８、９の変位寸法量ｄｌ、ｄｒ（左右関節駆動軸変数）から、関節角θ１、θ２を求め、さらに、第１ユニットのユニット先端位置姿勢を求めるものである。ユニット先端位置姿勢とは、第１ユニットの特定の位置（例えば、ＣＣＤカメラのレンズの位置）の座標軸の距離及び角度から第１ユニットの位置姿勢を表すものである。
【００２２】
前記のｄｌは、左側（left）の関節駆動軸８の変位寸法量、ｄｒは、右側（right）の関節駆動軸９の変位寸法量を示している。さらに変位寸法量ｄｌ、ｄｒを図１、図２を用いて具体的に説明する。変位寸法量ｄｌ、ｄｒは、折れ曲がった状態を含めて、両雌ネジ部材１６，１６間の関節駆動軸８、９の軸要素８ａ，８ｂ，９ａ，９ｂの長さ（もしくは下記基準姿勢における基準寸法に対する変化量）であるが、第１ユニット１と、第２ユニット２間の関節駆動軸８，９の軸要素８ａ，８ｂ，９ａ，９ｂの長さ（もしくは下記基準姿勢における基準寸法に対する変化量）と言い表すこともできる。この変位寸法量ｄｌ、ｄｒは、前記の説明のとおり、関節駆動モータ６、６により、関節駆動軸８、９を正逆方向に回転することにより、両ユニット間の全長（以下、単に寸法という）は変化する。前記のように、図２の雌ネジ部材１６，１６に螺合している関節駆動軸８，９には右ネジ１５と左ネジ１４が刻まれているので、関節駆動モータ６、６の回転により、ユニバーサルジョイント１１，１２を挟んで連結されたそれぞれの軸要素８ａ，８ｂ，９ａ，９ｂは、同寸法だけ対称（軸要素がそれぞれ逆方向）に両ユニット１，２から出入りする。第１ユニット１と第２ユニット２が、図３において、第１ユニットを点線で示すように一直線状の姿勢（以下、基準姿勢という）の場合の両ユニット１，２間の関節駆動軸８，９の寸法を基準寸法とすると、関節駆動モータ６，６の例えば正回転により軸要素８ａ，８ｂ，９ａ，９ｂがそれぞれのユニット１，２に退いていく（中に入る状態）とすると、両ユニット１，２間の関節駆動軸８，９の寸法は、前記基準寸法より短くなる。逆に、関節駆動モータ６，６を逆回転すると、軸要素８ａ，８ｂ，９ａ，９ｂはそれぞれのユニット１，２から同じ寸法だけ出てくる（中から出てくる状態）ので、両ユニット１，２間の関節駆動軸８，９の寸法は前記の基準寸法より長くなる。この両ユニット１，２間の関節駆動軸８，９の軸要素８ａ，８ｂ，９ａ，９ｂの伸縮する寸法を変位寸法量ｄｌ、ｄｒと言う。この変位寸法量ｄｌ、ｄｒは、第１ユニットの先端位置姿勢を求める際に、数式を解析する場合は左右の関節駆動軸の左右関節駆動軸変数と言う。
この関節駆動軸８，９の左右関節駆動軸変数ｄｌ、ｄｒから、後述する関数関係を解析することにより、関節角θ１、θ２を求めることができる。関節角θ１は、図３に示すように、第１ユニット１が、上下方向に回転した場合の回転角である。より正確には、推進駆動軸５のユニバーサルジョイント１３からＹ軸方向に垂直に伸びる軸線２０（仮想Ｙ軸基準線）が、変位寸法量ｄｌ、ｄｒの変化に伴って、前記ユニバーサルジョイント１３の位置におけるＺ軸線を中心として回動した場合の回転角である。図３は、第１ユニット１がユニバーサルジョイント１３のＺ軸線を中心にθ１だけ回転して上方に持上げられた状態を示す正面図である。関節角θ２は、変位寸法量ｄｌ、ｄｒの変化に伴って、第１ユニット１が、仮想Ｙ軸基準線２０を中心として、左右方向に回動した場合の回転角である。図４の平面図は、図３の正面図の状態から第１ユニット１がさらに仮想Ｙ軸基準線２０を中心にしてθ２だけ左方向に回転した状態を示している。
【００２３】
次に、関節駆動軸の変位寸法量ｄｌ、ｄｒと、関節角θ１、θ２との関係について説明する。
１）関節駆動軸要素８ａ，８ｂの変位寸法量ｄｌと、同９ａ，９ｂの変位寸法量ｄｒとが同じだけ基準寸法より短い状態。
【００２４】
第１ユニット１が、基準姿勢の状態からユニバーサルジョイント１３のＺ軸線を中心に上方に曲がる。変位寸法量ｄｌ、ｄｒの変化量が大きくなると、関節角θ１は大きくなり、変位寸法量ｄｌ、ｄｒの変化量が小さくなると、関節角θ１は小さくなる。
２）関節駆動軸要素８ａ，８ｂの変位寸法量ｄｌと、同９ａ，９ｂが変位寸法量ｄｒとが同じだけ基準寸法より長い状態。
【００２５】
第１ユニット１が、基準姿勢の状態からユニバーサルジョイント１３のＺ軸を中心に下方に曲がる。変位寸法量ｄｌ、ｄｒの変化量が大きくなると、関節角−θ１の絶対値は大きくなり、変位寸法量ｄｌ、ｄｒの変化量が小さくなると、関節角−θ１の絶対値は小さくなる。
３）基準寸法より、左側関節駆動軸要素８a,８ｂの変位寸法量ｄｌが短く、右側関節駆動軸要素９ａ，９ｂの変位寸法量ｄｒが長い状態。
【００２６】
第１ユニット１が、基準姿勢の状態から仮想Ｙ軸基準線２０を中心に左に曲がる。変位寸法量ｄｌ、ｄｒの変化量が大きくなると関節角θ２は大きくなり、変位寸法量ｄｌ、ｄｒの変化量が小さくなると、関節角θ２は小さくなる。
４）基準寸法より、左側関節駆動軸要素８ａ，８ｂの変位寸法量ｄｌが長く、右側関節駆動軸要素９ａ，９ｂの変位寸法量ｄｒが短い状態。
【００２７】
第１ユニット１が、基準姿勢の状態から仮想Ｙ軸基準線２０を中心に右に曲がる。変位寸法量ｄｌ、ｄｒの変化量が小さくなると関節角−θ２の絶対値は小さくなり、変位寸法量ｄｌ、ｄｒの変化量が大きくなると、関節角−θ２の絶対値は大きくなる。
【００２８】
以上の１）２）３）４）の基本姿態をさらに組み合わせた、１）＋３）、１）＋４）、２）＋３）、２）＋４）の姿勢ができる。この姿勢は第１ユニット１がＺ軸線を中心に上下方向に曲がった状態から、さらに、左右の何れかの方向に回転して捻れた状態の組み合わせである。
【００２９】
上記の変位寸法量ｄｌ、ｄｒと関節角θ１、θ２とは幾何学的関係から次のような関数関係となる。
(ｄｌ、ｄｒ)＝ｇ（θ１、θ２） 式Ｉ
この式Ｉの逆関数は次の式IIのようになり、ｄｌ、ｄｒより関節角θ１、θ２を求める際に用いる順運動学関係である。
【００３０】
(θ１、θ２)＝ｇ-１(ｄｌ、ｄｒ) 式II
上記の式Ｉ、IIのｇは関数を表し、そのパラメータ値は、能動連結機構の種々の寸法等の要素によって定まる。例えば、推進駆動軸５と関節駆動軸８，９の寸法及び関節駆動軸８，９と推進駆動軸５との距離関係等から導き出されるものである。（ｄｌ、ｄｒ）は前記のように関節駆動モータ６の回転数をエンコーダ１０によりカウントすることにより判明する。左右関節駆動軸変数（ｄｌ、ｄｒ）よりｇの逆関数（式II）から関節角θ１、θ２を求めることができる。
【００３１】
また、能動連結機構を表す数学モデルとして関節座標系（ｘi,ｙi,ｚi）と関節動作を表す関節角θ１、θ２とを適正に設定することにより、基準座標系（ｘR,ｙR,ｚR）やユニット先端座標系（ｘE,ｙE,ｚE）を含めた座標系間の同時変換関係を表す行列
ｉ-１Ｔi＝ｉ-１Ｔi(θi)
の各要素を用いることにより、次のようなθ１、θ２から第１ユニット先端位置姿勢までの順運動学関係を求めることができる。
【００３２】
（ｘE,ｙE,ｚE,θE,φE,ψE）＝ｆ(θ１、θ２) 式III
上記式IIIの（ｘE,ｙE,ｚE）は、第１ユニット１の先端位置(Eed)における先端座標系の基準座標系に対する距離を示し、（θE,φE,ψE）は、先端座標系の基準座標系に対する角度を示している。この距離（ｘE,ｙE,ｚE）と角度（θE,φE,ψE）により第１ユニットの先端位置姿勢を表している。
【００３３】
また、式IIIのｆは関数（function）を示し、ユニット１、２や能動連結機構の寸法等によって定まるものである。
以上のような式Ｉ、式II、式IIIを用いて、左右関節駆動軸変数（関節駆動軸の変位寸法量）ｄｌ、ｄｒから、現在の第１ユニットの先端位置姿勢を求めることができる。
【００３４】
次に、第１ユニット１の現在の先端位置姿勢をパーソナルコンピュータにより求める場合を図５のフローチャートと、図７のブロック図を用いて説明する。
図７のＩ/Ｆ，ＣＰＵ，ＲＡＭ，ＲＯＭ，ＬＣＤ（液晶ディスプレイ）等はパーソナルコンピュータを示しているが、制御用の特別なマイクロコンピュータを用いたものであってもよい。図７において、６、６は関節駆動モータ、７は推進駆動モータ、１０，１０はエンコーダ（ＥＮ）を示している。
【００３５】
ステップ１；第１ユニット１の先端位置姿勢のｎ周期目の検出のスタートを示している。キーボードからの指令により、ＲＯＭ内に格納されているプログラムにより先端位置姿勢の検出を開始する。
【００３６】
ステップ２；左側の関節駆動軸８に設けられているエンコーダ１０(L)から左側関節駆動軸８の変位寸法量（ｄｌ）を検出する。即ち、左側のエンコーダ１０（L）の正逆方向の回転数から関節駆動軸８の現在の寸法を割り出し変位寸法量（ｄｌ）を検出する。
【００３７】
ステップ３；右側の関節駆動軸９に設けられているエンコーダ１０(R)から右側関節駆動軸９の変位寸法量（ｄｒ）を検出する。即ち、右側のエンコーダ１０（R）の正逆方向の回転数から関節駆動軸９の現在の寸法を割り出し変位寸法量（ｄｒ）を検出する。
【００３８】
ステップ４、５；前記ステップ２で検出した変位寸法量（ｄｌ）と、前記ステップ３で検出した変位寸法量（ｄｒ）とからＺ軸回りの関節角θ１と、仮想Ｙ軸基準線回りの関節角θ２とを算出する。この関節角θ１θ２を算出する式IIの関数関係は前記の通りである。
【００３９】
ステップ６；前記ステップ４、５で求めた関節角θ１θ２から第１ユニット１の先端位置姿勢（ｘE,ｙE,ｚE,θE,φE,ψE）を算出する。この第１ユニット１の先端位置姿勢は式IIIから求めることができることは、前記の通りである。
【００４０】
ステップ７；前記のステップ６で求めた結果をＬＣＤに表示する。第１ユニット１の先端位置姿勢（ｘE,ｙE,ｚE,θE,φE,ψE）を出力して、第１ユニット１の先端位置姿勢を画像としてＬＣＤに表示する。
【００４１】
ステップ８；ステップ７により求めた第１ユニット１の先端位置姿勢からｎ＋１周期目の検出に移行する。ｎ＋１周期目以降も同様にして刻々変化する第１ユニット１の先端位置姿勢を算出し、ＬＣＤに表示することができる。
II）第１ユニット先端目標位置実現のための変位寸法量を求める。（逆運動学関係）
逆運動学関係は、第１ユニット１の先端位置姿勢から関節駆動軸８，９の変位寸法量ｄｌ、ｄｒへの関係を与えるものであり、操縦者が所望する第１ユニット１の先端位置姿勢を実現するために必要な関節駆動軸８，９の変位寸法量ｄｌ、ｄｒを求めるのに用いる。即ち、第１ユニット１を目標とする位置姿勢にするために左右の関節駆動軸８，９の寸法（ｄｌ、ｄｒ）をどれだけにすればよいかを解析するものである。そのためには、第１ユニットの先端位置姿勢の実現に必要な回転角θｄ１、θｄ２を求め、さらに必要な関節駆動軸８，９の変位寸法量（左右関節駆動軸変数）ｄｄｌ、ｄｄｒを求める逆運動学関係を解析的に解くことにより、第１ユニット１の先端目標位置姿勢を実現する。
【００４２】
前記の順運動学関係の式IIIの逆関係である逆運動学関係は次の通り表されるとする。
(θ１、θ２)＝ｆ-１(ｘE,ｙE,ｚE,θE,φE,ψE) 式Ｖ
いま、第１ユニット１の先端目標位置姿勢を次のように与えられるとする。
【００４３】
（ｘE,ｙE,ｚE,θE,φE,ψE）ｄ 式VI
この式VIに逆運動学関係式Ｖを当てはめると、目標回転角θｄ１、θｄ２が求められる。
【００４４】
(θｄ１、θｄ２)＝ｆ-１（ｘE,ｙE,ｚE,θE,φE,ψE）ｄ
さらに、回転角θｄ１、θｄ２から関数式Ｉを用いて左右関節駆動軸変数（関節駆動軸の変位寸法量）ｄｄｌ、ｄｄrを求めることができる。
【００４５】
(ｄｄl、ｄｄr)＝ｇ（θｄ1、θｄ2）
このようにして求めた関節駆動軸の変位寸法量ｄｄl、ｄｄrに基づいて関節駆動モータ６を回動して回転駆動軸８，９の両ユニット１，２間の寸法をこの変位寸法量ｄｄl、ｄｄrに適合するように制御すれば、第１ユニット１の先端位置姿勢を実現することができる。
【００４６】
次に、前記の説明のうち、第１ユニットの先端位置姿勢を実現するための制御方法について、図６のフローチャートと、図７のブロック図を用いて説明する。図７のＩ/Ｆ，ＣＰＵ，ＲＡＭ，ＲＯＭ，ＬＣＤ等はパーソナルコンピュータを示しているが、制御用の特別なコントローラであってもよい。
【００４７】
先ず、前記のように、逆運動学関係により第１ユニット１の先端位置姿勢（ｘE,ｙE,ｚE,θE,φE,ψE）ｄを実現するために必要な第１ユニット１の回転角θｄ１、θｄ２を導き出し、ユニバーサルジョイント１３のＺ軸線を中心軸として第１ユニット１が上下に回動する回転角θｄ１と、仮想Ｙ軸基準線２０を中心軸として第１ユニット１が左右に回動する回転角θｄ２に分解する。分解した回転角θｄ１のＺ軸線を中心軸とする動作指令と、回転角θｄ２の仮想Ｙ軸基準線２０を中心軸とする動作指令を出す。（ステップ１）
次に、（ステップ１）で分解した回転角θｄ１から左側関節駆動軸８の関節駆動変位寸法量ｄｄl 1、と右側関節駆動軸９の関節駆動変位寸法量ｄｄr 1とに分解する。（ステップ２）
さらに、（ステップ１）で分解した回転角θｄ２から左側関節駆動軸８の関節駆動変位寸法量ｄｄl 2、と右側関節駆動軸９の関節駆動変位寸法量ｄｄr 2とに分解する。（ステップ３）
次に、ステップ２とステップ３で分解した関節駆動変位寸法量ｄｄl 1、関節駆動変位寸法量ｄｄｒ1、関節駆動変位寸法量ｄｄl 2、関節駆動変位寸法量ｄｄｒ 2を用いて、ｄｄl 1とｄｄl 2とを足し合わせて左側関節駆動軸８の目標変位寸法量ｄｄlを得（ステップ４）、ｄｄｒ1 とｄｄｒ 2とを足し合わせて右側関節駆動軸９の目標変位寸法量ｄｄｒを得（ステップ５）、それらをサーボ制御により実現する。
【００４８】
図６の左端のシーケンスは、左側関節駆動軸８の目標変位寸法量ｄｄｌ を実現するためのサーボ制御をデジタルコンピュータを用いて離散的に実現する場合を示している。このサーボ制御（Ｓ.Ｃ.Ｉ）はステップ４によって得られる左側関節駆動軸８の目標変位寸法量ｄｄｌと、現在の左側の関節駆動軸８の変位寸法量ｄｌを比較する。左側関節駆動軸８の目標変位寸法量ｄｄｌと、現在の左側の関節駆動軸８の変位寸法量ｄｌとの差が０の場合、即ち左側関節駆動軸８の変位寸法量ｄｌを変える必要がない場合は現在のｄｌを保持し次ぎの制御周期に移る。
もし、左側関節駆動軸の目標変位寸法量ｄｄｌと、現在の左側の関節駆動軸８の変位寸法量ｄｌ間に誤差がある場合は、その誤差を修正する制御を行う。即ち、エンコーダ１０（ＥＮ）により関節駆動軸８の回転数をカウントして、得られる現在の変位寸法量ｄｌを、ステップ４で得られる左側関節駆動軸の目標変位寸法量ｄｄｌと比較する。そして、その差に対してゲイン（誤差の変換・増幅率）Ｃの定めに基づき増幅し、さらにドライバによりモータ駆動入力に増幅し、左側のアクチュエータ（関節駆動モータ６）に通電し、左側関節駆動軸の目標変位寸法量ｄｄｌと、現在の左側関節駆動軸の変位寸法量ｄｌとの間の差が０となる方向に回転する。このようなサーボ制御により、左側関節駆動軸の目標変位寸法量ｄｄｌと、現在の左側関節駆動軸の変位寸法量ｄｌの誤差を０にする。
シーケンス（Ｓ.Ｃ.Ｉ）と同時に右隣りの第２の制御シーケンス（Ｓ.Ｃ.II）を同様に実施して、ステップ５で得られる右側関節駆動軸の目標変位寸法量ｄｄｒと、現在の右側の関節駆動軸の変位寸法量ｄｒを比較して、誤差がある場合はアクチュエータにより修正して誤差をなくすよう制御を行う。
【産業上の利用可能性】
【００４９】
本発明の応用例の一例として震災や災害時に建造物等の瓦礫の中を探査するための探査用装置に応用することができる。
【図面の簡単な説明】
【００５０】
【図１】本発明の能動連結機構の斜視図である。
【図２】本発明の能動連結機構の部分構成図である。
【図３】本発明の能動連結機構の作動原理を説明するための説明図である。
【図４】本発明の能動連結機構の作動原理を説明するための説明図である。
【図５】本発明の能動連結機構の位置姿勢の検出方法を示すフローチャートである。
【図６】本発明の能動連結機構の制御方法を示すフローチャートである。
【図７】本発明の能動連結機構の検出部・制御部等のブロック図である。
【符号の説明】
【００５１】
１ 第１ユニット
２ 第２ユニット
３ ケーブル
４ 無限軌道
５ 推進駆動軸
６ 関節駆動装置
７ 推進駆動装置
８ 関節駆動軸
８ａ 軸要素
８ｂ 軸要素
９ 関節駆動軸
９ａ 軸要素
９ｂ 軸要素
１０ エンコーダ
１１ ユニバーサルジョイント
１２ ユニバーサルジョイント
１３ ユニバーサルジョイント
１４ 左ネジ
１５ 右ネジ
１６ 雌ネジ部材
１７ 基台
１８ 基台
２０ 仮想Ｙ軸基準線
【特許請求の範囲】
【請求項１】
先行する第１ユニットと後行する第２ユニットとを連結し、第２ユニットから第１ユニットへ駆動力を伝達する能動連結機構において、
この能動連結機構は、一対の関節駆動軸と推進駆動軸からなり、前記各軸の中央部が三次元ジョイントにより接合され、
前記関節駆動軸は、両端部に互いに逆向きのネジが形成されて回動可能に軸支され、回動により三次元ジョイントで接合された関節駆動軸の各軸要素が各々のユニットから対称に出退して、各ユニット間の関節駆動軸の長さを可変とし、
推進駆動軸は、両端が各ユニットに回転可能に軸支されると共に、各ユニット間の長さは固定され、中央部の三次元ジョイントを中心として第１ユニットを操作する、ことを特徴とする能動連結機構。
【請求項２】
一対の関節駆動軸と推進駆動軸が逆２等辺三角形に配置されたことを特徴とする請求項１に記載の能動連結機構。
【請求項３】
中央部が三次元ジョイントにより接合された一対の関節駆動軸と推進駆動軸からなり、先行する第１ユニットと後行する第２ユニットとを連結し、第２ユニットから第１ユニットへ駆動力を伝達する能動連結機構の位置姿勢の検出方法において、
前記一対の関節駆動軸の寸法を検出し、この検出した関節駆動軸の寸法から、推進駆動軸の三次元ジョイントの関節角を求めた後、第１ユニットの先端位置姿勢を求めることを特徴とする能動連結機構の位置姿勢の検出方法。
【請求項４】
中央部が三次元ジョイントにより接合された一対の関節駆動軸と推進駆動軸からなり、先行する第１ユニットと後行する第２ユニットとを連結し、第２ユニットから第１ユニットへ駆動力を伝達する能動連結機構の制御方法において、
所望の第１ユニットの先端位置姿勢を決定し、この先端位置姿勢の実現に必要な推進駆動軸の関節角を求め、この推進駆動軸の三次元ジョイントの関節角から関節駆動軸の変位寸法量を求め、この変位寸法量に一対の関節駆動軸の寸法を適合させる操作を行うことを特徴とする能動連結機構の制御方法。
【請求項１】
先行する第１ユニットと後行する第２ユニットとを連結し、第２ユニットから第１ユニットへ駆動力を伝達する能動連結機構において、
この能動連結機構は、一対の関節駆動軸と推進駆動軸からなり、前記各軸の中央部が三次元ジョイントにより接合され、
前記関節駆動軸は、両端部に互いに逆向きのネジが形成されて回動可能に軸支され、回動により三次元ジョイントで接合された関節駆動軸の各軸要素が各々のユニットから対称に出退して、各ユニット間の関節駆動軸の長さを可変とし、
推進駆動軸は、両端が各ユニットに回転可能に軸支されると共に、各ユニット間の長さは固定され、中央部の三次元ジョイントを中心として第１ユニットを操作する、ことを特徴とする能動連結機構。
【請求項２】
一対の関節駆動軸と推進駆動軸が逆２等辺三角形に配置されたことを特徴とする請求項１に記載の能動連結機構。
【請求項３】
中央部が三次元ジョイントにより接合された一対の関節駆動軸と推進駆動軸からなり、先行する第１ユニットと後行する第２ユニットとを連結し、第２ユニットから第１ユニットへ駆動力を伝達する能動連結機構の位置姿勢の検出方法において、
前記一対の関節駆動軸の寸法を検出し、この検出した関節駆動軸の寸法から、推進駆動軸の三次元ジョイントの関節角を求めた後、第１ユニットの先端位置姿勢を求めることを特徴とする能動連結機構の位置姿勢の検出方法。
【請求項４】
中央部が三次元ジョイントにより接合された一対の関節駆動軸と推進駆動軸からなり、先行する第１ユニットと後行する第２ユニットとを連結し、第２ユニットから第１ユニットへ駆動力を伝達する能動連結機構の制御方法において、
所望の第１ユニットの先端位置姿勢を決定し、この先端位置姿勢の実現に必要な推進駆動軸の関節角を求め、この推進駆動軸の三次元ジョイントの関節角から関節駆動軸の変位寸法量を求め、この変位寸法量に一対の関節駆動軸の寸法を適合させる操作を行うことを特徴とする能動連結機構の制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【公開番号】特開２００９−３０７４８（Ｐ２００９−３０７４８Ａ）
【公開日】平成２１年２月１２日（２００９．２．１２）
【国際特許分類】
【出願番号】特願２００７−１９６８３５（Ｐ２００７−１９６８３５）
【出願日】平成１９年７月３０日（２００７．７．３０）
【国等の委託研究の成果に係る記載事項】（出願人による申告）国等の委託研究の成果に係る特許出願（平成１８年度、独立行政法人 防災科学技術研究所、「大都市大震災軽減化特別プロジェクト レスキューロボット等次世代防災基盤技術の開発」委託研究、産業再生法第３０条の適用を受ける特許出願。）
【出願人】（５０７２５５９１６）
【出願人】（５９１１４８０９３）財団法人京都高度技術研究所 (2)
【出願人】（５０３１３２２８０）特定非営利活動法人 国際レスキューシステム研究機構 (6)
【Ｆターム（参考）】
【公開日】平成２１年２月１２日（２００９．２．１２）
【国際特許分類】
【出願日】平成１９年７月３０日（２００７．７．３０）
【国等の委託研究の成果に係る記載事項】（出願人による申告）国等の委託研究の成果に係る特許出願（平成１８年度、独立行政法人 防災科学技術研究所、「大都市大震災軽減化特別プロジェクト レスキューロボット等次世代防災基盤技術の開発」委託研究、産業再生法第３０条の適用を受ける特許出願。）
【出願人】（５０７２５５９１６）
【出願人】（５９１１４８０９３）財団法人京都高度技術研究所 (2)
【出願人】（５０３１３２２８０）特定非営利活動法人 国際レスキューシステム研究機構 (6)
【Ｆターム（参考）】
[ Back to top ]