JP4413251B2

JP4413251B2 - Active coupling mechanism, position and orientation detection method and control method thereof

Info

Publication number: JP4413251B2
Application number: JP2007196835A
Authority: JP
Inventors: 心知鄭
Original assignee: 心知鄭; 財団法人京都高度技術研究所; 特定非営利活動法人国際レスキューシステム研究機構
Priority date: 2007-07-30
Filing date: 2007-07-30
Publication date: 2010-02-10
Anticipated expiration: 2027-07-30
Also published as: JP2009030748A

Description

本発明は、先進する第１ユニットに、連結する後方の第２ユニットから駆動力を伝達する能動連結機構及びその位置姿勢の状態を検出すると共に、第１ユニットを所望の姿勢に制御するその制御方法に関し、本発明は、種々の装置に応用できるが、その応用例の一例として震災や災害時に建造物等の瓦礫の中を探査するための探査用装置や原子力発電所等の危険な領域での監視システム等に応用することのできる能動連結機構及びその位置姿勢の検出方法及びその制御方法に関する。 The present invention detects an active coupling mechanism that transmits a driving force from a second unit that is connected to an advanced first unit and a state of the position and orientation of the active coupling mechanism and controls the first unit to a desired posture. Regarding the method, the present invention can be applied to various devices. As an example of the application, a dangerous region such as an exploration device or a nuclear power plant for exploring a debris such as a building at the time of an earthquake disaster or a disaster. The present invention relates to an active coupling mechanism that can be applied to a monitoring system and the like, and a position and orientation detection method and control method thereof.

前記のように、震災や災害時に建造物等の瓦礫の中を探査する際、生存者の救出のために、種々の探査用装置が開発されている。その一例として特開２００４−３５０８８９号公報に簡易型探査用レスキュー装置が掲載されている。この特許文献記載のレスキュー装置は先端部に車輪を有する探査部を備え、駆動ハンドルから駆動用ワイヤを介して車輪に駆動力を伝達している。さらに、２本の操舵用ワイヤがケーブル部内に配置され、その先端は前記探査部の背面側の左右両側にそれぞれ１本ずつ接続され、他端には操舵用ハンドルが接続されている。操縦者はケーブル部の後端部を把持しながら、操舵用ハンドルを牽引することにより探査部の方向を左右に操縦している。
特開２００４−３５０８８９号公報 As described above, various exploration devices have been developed to rescue survivors when exploring rubble such as buildings during an earthquake or disaster. As an example, Japanese Patent Application Laid-Open No. 2004-350889 discloses a simple exploration rescue device. The rescue device described in this patent document includes an exploration portion having a wheel at the tip, and transmits a driving force from the driving handle to the wheel via a driving wire. Further, two steering wires are arranged in the cable portion, one end thereof is connected to each of the left and right sides of the back side of the exploration portion, and a steering handle is connected to the other end. The driver steers the steering handle to the left and right by pulling the steering handle while grasping the rear end of the cable portion.
JP 2004-350889 A

しかしながら、前記従来の探査用レスキュー装置の探査部は瓦礫の中に完全に入り込んで隠れた状態にあることもあり、先端の探査部の姿勢が左右方向、上下方向にどれだけが傾いているのかその姿勢を正確につかむことができず、カメラに写し出された映像もどのような角度で写し出されているのか判然としない。しかも、現在の姿勢から所望の角度だけ探査部の角度を変えようとしても探査部が目視できないため、不可能であった。かりに、探査部の姿勢を検知するとすれば、探査部の角度を検出する角度検出センサーを探査部の外部等に装備する必要がある。さらに、２本の操舵用ワイヤを牽引しても探査部は左右方向の操縦しかできず、探査部を上方向に持上げて障害物を乗り越えるようにするためには、さらに操舵用ワイヤを１本増やさなければならない。また、狭隘な瓦礫等の中に入り込む場合、探査部の小型化の必要性、それによる探査部の搭載スペースの制約によって、探査部と同一のユニット上に動力機構が搭載できず、何らかの形で連結する別の動力発生ユニットから駆動に必要な動力を得る必要がある。 However, the exploration part of the conventional exploration rescue device may be completely hidden in the debris and hidden, and how much the position of the exploration part at the tip is tilted in the horizontal and vertical directions? The posture cannot be grasped accurately, and it is not clear at what angle the image projected on the camera is projected. In addition, even if an attempt is made to change the angle of the exploration part by a desired angle from the current posture, the exploration part cannot be visually observed, which is impossible. If the attitude of the exploration unit is detected, an angle detection sensor for detecting the angle of the exploration unit needs to be provided outside the exploration unit. In addition, even if the two steering wires are pulled, the exploration part can only be operated in the left-right direction, and in order to lift the exploration part upward and get over obstacles, one more steering wire is required. It must be increased. In addition, when entering into narrow debris, the power mechanism cannot be mounted on the same unit as the exploration unit due to the necessity of downsizing the exploration unit and constraining the mounting space of the exploration unit. It is necessary to obtain power necessary for driving from another power generation unit to be connected.

本発明は上記課題を解決するためになされたもので、機構上の最少構成を有しながら、後方動力ユニットから先方駆動ユニットへ駆動力を伝達するとともに、目視することのできない先行する第１ユニットの現在の位置姿勢を検出することを可能とし、しかもその第１ユニットの現在の姿勢から所望の角度の姿勢に変更することができる能動連結機構及びその位置姿勢の検出方法及びその制御方法を提供することを目的とするものである。 The present invention has been made to solve the above-described problems, and transmits the driving force from the rear power unit to the front drive unit while having the minimum configuration on the mechanism, and the preceding first unit that cannot be visually observed. Provided is an active coupling mechanism that can detect the current position and orientation of the first unit, and can change the current posture of the first unit from the current posture to a desired angle, and a method and a control method for the position and orientation It is intended to do.

本発明の能動連結機構は、先行する第１ユニットと後行する第２ユニットとを連結し、第２ユニットから第１ユニットへ駆動力を伝達する能動連結機構であって、この能動連結機構は、一対の関節駆動軸と推進駆動軸からなり、前記各軸の中央部が三次元ジョイントにより接合され、前記関節駆動軸は、両端部に互いに逆向きのネジが形成されて回動可能に軸支され、回動により三次元ジョイントで接合された関節駆動軸の各軸要素が各々のユニットから対称に出退して、各ユニット間の関節駆動軸の長さを可変とし、推進駆動軸は、両端が各ユニットに回転可能に軸支されると共に、各ユニット間の長さは固定され、中央部の三次元ジョイントを中心として第１ユニットを操作することを特徴とする。 The active coupling mechanism of the present invention is an active coupling mechanism that couples a preceding first unit and a following second unit and transmits a driving force from the second unit to the first unit. The joint drive shaft and the propulsion drive shaft are composed of a central part of the shafts joined by a three-dimensional joint, and the joint drive shafts are pivotable by forming opposite-direction screws at both ends. Each shaft element of the joint drive shaft that is supported and joined by a three-dimensional joint by turning is symmetrically withdrawn from each unit, the length of the joint drive shaft between each unit is variable, and the propulsion drive shaft is Both ends are rotatably supported by each unit, the length between each unit is fixed, and the first unit is operated around a three-dimensional joint at the center.

また、本発明の能動連結機構は、一対の関節駆動軸と推進駆動軸が逆２等辺三角形に配置されたことを特徴とする。
本発明の上記構成により、一対の関節駆動軸の寸法を検出することにより、推進駆動軸の三次元ジョイントの関節角を求め、さらに第１ユニットの先端位置姿勢を求めることができる。 Further, the active coupling mechanism of the present invention is characterized in that a pair of joint drive shafts and propulsion drive shafts are arranged in an inverted isosceles triangle.
According to the above configuration of the present invention, by detecting the dimensions of the pair of joint drive shafts, the joint angle of the three-dimensional joint of the propulsion drive shaft can be obtained, and the tip position / posture of the first unit can be obtained.

さらに、本発明の上記構成により、先端位置姿勢の実現に必要な推進駆動軸の関節角を求め、この推進駆動軸の三次元ジョイントの関節角から関節駆動軸の寸法を求め、この関節駆動軸寸法に一対の関節駆動軸を適合させることにより、第１ユニットを所望の先端位置姿勢にすることができる。 Further, according to the above configuration of the present invention, the joint angle of the propulsion drive shaft necessary for realizing the tip position and posture is obtained, the dimension of the joint drive shaft is obtained from the joint angle of the three-dimensional joint of the propulsion drive shaft, and the joint drive shaft By adapting the pair of joint drive shafts to the dimensions, the first unit can be in a desired tip position and posture.

さらに、本発明の上記構成により、第１ユニット先端位置姿勢の検出と制御が容易にできるともに、前記各軸の中央部が前記三次元ジョイントにより結合されていても、本発明の能動連結機構で連結される第１ユニットと第２ユニットとの間は各駆動軸線を中心に相捻れるような運動の自由度が発生することはなく、各駆動軸により第２ユニットから第１ユニットへ駆動力を伝達することができる。 Furthermore, the above configuration of the present invention makes it easy to detect and control the first unit tip position and orientation, and even if the central portion of each axis is coupled by the three-dimensional joint, the active coupling mechanism of the present invention can be used. There is no freedom of movement between the first unit and the second unit to be twisted about each drive axis, and the drive force from the second unit to the first unit by each drive axis. Can be transmitted.

さらに、本発明の能動連結機構は、動作途中のどの状態においても、特異姿勢となることがない。即ち、一対の関節駆動軸と推進駆動軸全体での自由度が縮退して第１ユニットの動きが取れなくなることはない。さらに、一対の関節駆動軸と推進駆動軸全体で所望外の自由度が発生して第１ユニットを所定の姿勢で固定できなくなることはない。
また、本発明の能動連結機構の位置姿勢の検出方法は、中央部が三次元ジョイントにより接合された一対の関節駆動軸と推進駆動軸からなり、先行する第１ユニットと後行する第２ユニットとを連結し、第２ユニットから第１ユニットへ駆動力を伝達する能動連結機構の位置姿勢の検出方法において、前記一対の関節駆動軸の寸法を検出し、この検出した関節駆動軸の寸法から、推進駆動軸の三次元ジョイントの関節角を求めた後、第１ユニットの先端位置姿勢を求めることを特徴とする。 Furthermore, the active coupling mechanism of the present invention does not take a unique posture in any state during operation. That is, the degree of freedom in the pair of joint drive shafts and the entire propulsion drive shaft is not reduced and the first unit cannot be moved. Furthermore, an undesired degree of freedom does not occur in the entire pair of joint drive shaft and propulsion drive shaft, and the first unit cannot be fixed in a predetermined posture.
In addition, the position and orientation detection method of the active coupling mechanism according to the present invention includes a pair of joint drive shafts and propulsion drive shafts whose central portions are joined by a three-dimensional joint, and a second unit following the preceding first unit. And detecting the dimensions of the pair of joint drive shafts from the detected dimensions of the joint drive shafts in the method for detecting the position and orientation of the active connection mechanism that transmits the driving force from the second unit to the first unit. After obtaining the joint angle of the three-dimensional joint of the propulsion drive shaft, the tip position / posture of the first unit is obtained.

また、本発明の能動連結機構の制御方法は、中央部が三次元ジョイントにより接合された一対の関節駆動軸と推進駆動軸からなり、先行する第１ユニットと後行する第２ユニットとを連結し、第２ユニットから第１ユニットへ駆動力を伝達する能動連結機構の制御方法において、所望の第１ユニットの先端位置姿勢を決定し、この先端位置姿勢の実現に必要な推進駆動軸の関節角を求め、この推進駆動軸の三次元ジョイントの関節角から関節駆動軸の変位寸法量を求め、この関節駆動軸の変位寸法量に一対の関節駆動軸の寸法を適合させる操作を行うことを特徴とする。 In addition, the control method of the active connection mechanism of the present invention comprises a pair of joint drive shaft and propulsion drive shaft, the central part of which is joined by a three-dimensional joint, and connects the preceding first unit and the following second unit. Then, in the control method of the active coupling mechanism for transmitting the driving force from the second unit to the first unit, the tip position / posture of the desired first unit is determined, and the joint of the propulsion drive shaft necessary for realizing the tip position / posture Obtaining the angle, obtaining the displacement dimension amount of the joint drive shaft from the joint angle of the three-dimensional joint of this propulsion drive shaft, and performing an operation of adapting the dimensions of the pair of joint drive shafts to the displacement dimension amount of this joint drive shaft Features.

本発明の上記構成により、先行する第１ユニットで必要な駆動力をその後方に連結される第２ユニットから伝達することができる。さらに、目視できない先行する第１ユニットの姿勢を、一対の関節駆動軸の寸法から正確に割り出すことができる。さらに、目視できない第１ユニットを所望の姿勢に制御することができる。 According to the above configuration of the present invention, the driving force necessary for the preceding first unit can be transmitted from the second unit connected to the rear thereof. Furthermore, the posture of the preceding first unit that cannot be visually observed can be accurately determined from the dimensions of the pair of joint drive shafts. Furthermore, it is possible to control the first unit that cannot be seen to a desired posture.

以下に、本発明を実施するための最良の形態について、図面を用いて説明する。
図１は、本発明の動力伝達機能を有する能動連結機構の概略斜視図である。この能動連結機構は、先行する第１ユニット１と、この第１ユニット１の後方に接合する第２ユニット２との間に配置されて、第２ユニット２の後端に接続されたケーブル３を介して制御装置を通して第１ユニット１の位置姿勢を検出したり、制御するものである。 The best mode for carrying out the present invention will be described below with reference to the drawings.
FIG. 1 is a schematic perspective view of an active coupling mechanism having a power transmission function of the present invention. This active coupling mechanism is arranged between the preceding first unit 1 and the second unit 2 joined to the rear of the first unit 1, and connects the cable 3 connected to the rear end of the second unit 2. The position and orientation of the first unit 1 are detected and controlled through the control device.

図１には図示していないが、第１ユニット１には、例えばＣＣＤカメラや温度や湿度等を測定するセンサーが搭載されている。ただし、この第１ユニット１には、電源（バッテリ）や自走するための駆動装置（具体的にはモータを含めた駆動機構）は搭載されていない。駆動力は第２ユニット２から伝達される。また、電力はケーブル３を通して送られている。第１ユニット１の外壁面には無限軌道（クローラ）４が取り付けられており、この無限軌道４により第１ユニット１が移動し、第２ユニットがそれに牽引されて移動できるように構成されている。なお、この図１では原理だけを説明するため、詳細な構成は省略してある。なお、無限軌道４は後述する推進駆動軸５からの駆動力に基づいて駆動するものである。 Although not shown in FIG. 1, for example, a CCD camera and a sensor for measuring temperature, humidity, and the like are mounted on the first unit 1. However, the first unit 1 is not equipped with a power source (battery) or a driving device for self-running (specifically, a driving mechanism including a motor). The driving force is transmitted from the second unit 2. Electric power is sent through the cable 3. An endless track (crawler) 4 is attached to the outer wall surface of the first unit 1. The first unit 1 is moved by the endless track 4, and the second unit can be pulled and moved by the endless track 4. . In FIG. 1, only the principle is described, and the detailed configuration is omitted. The endless track 4 is driven based on a driving force from a propulsion drive shaft 5 described later.

第２ユニット２には、関節駆動装置６（以下、関節駆動モータという）、推進駆動装置７（以下、推進駆動モータという）、関節駆動モータ６又は関節駆動軸８、９の回転数をカウントするエンコーダ１０（ＥＮ）等が備えられており、その他、第１ユニット１を支援するための装置が搭載されている。関節駆動モータ６は後述するように左右の関節駆動軸８、９を回転して第１ユニット１の姿勢を変えるための駆動力を発生させるためのモータである。推進駆動モータ７は第１ユニット１の無限軌道４に回転駆動力を与えるためのモータである。なお、ケーブル３は制御装置とユニット１、２間の信号のやり取りを行うと共に、第１ユニット１と第２ユニット２の電力を送電するためのケーブルである。 The second unit 2 counts the number of rotations of the joint driving device 6 (hereinafter referred to as a joint driving motor), the propulsion driving device 7 (hereinafter referred to as a propulsion driving motor), the joint driving motor 6 or the joint driving shafts 8 and 9. An encoder 10 (EN) and the like are provided, and in addition, a device for supporting the first unit 1 is mounted. The joint drive motor 6 is a motor for generating a drive force for rotating the left and right joint drive shafts 8 and 9 to change the posture of the first unit 1 as will be described later. The propulsion drive motor 7 is a motor for applying a rotational drive force to the endless track 4 of the first unit 1. The cable 3 is a cable for exchanging signals between the control device and the units 1 and 2 and transmitting the power of the first unit 1 and the second unit 2.

次に、能動連結機構の構成について説明する。能動連結機構は左右一対の関節駆動軸８、９と、１本の推進駆動軸５から構成されている。関節駆動軸８、９は第１ユニット１の姿勢を制御するものであり、推進駆動軸５は、前記のように第１ユニット１に回転駆動力を伝達するものである。この２本の関節駆動軸８、９と１本の推進駆動軸５との配置は、関節駆動軸８、９が上部に平行に所定間隔を開けて配置されており、この２本の関節駆動軸８、９に対して逆２等辺三角形となる位置に推進駆動軸５が配置されている。
関節駆動軸８、９は、図２に示すように、２本の軸要素８ａ、８ｂ及び軸要素９ａ，９ｂが中央部で三次元ジョイント１１、１２（ユニバーサルジョイント、自在継手、万能継手等で呼ばれる三次元方向に曲がる継ぎ手を言う。またスプリングのようなものであってもよい。以下、図示するユニバーサルジョイントの用語を用いて説明する）により連結され、このユニバーサルジョイント１１、１２を挟んでそれぞれの軸要素８ａ、８ｂ、９ａ、９ｂを上下、左右、斜め方向に曲げることができる。関節駆動軸８、９の両端部には、それぞれ逆方向の右ネジ１５と左ネジ１４が所定の長さだけ同ピッチで雄ネジとして刻まれている。さらに、この右ネジ１５と左ネジ１４に、それぞれ雌ネジ部材１６（ナット）が螺合している。これらの雌ネジ部材１６はそれぞれの第１ユニット１、第２ユニット２の基台１７、１８に固定されて移動は不可能である。これに対して、関節駆動軸８、９は、雌ネジ部材１６に螺合した右ネジ１５と左ネジ１４によって、ユニバーサルジョイント１１、１２を挟んで連結している２本の軸要素８ａ、８ｂ、９ａ、９ｂが、関節駆動軸８、９の正逆方向の回転により、対称（ユニバーサルジョイントを挟んで逆方向）に移動する。このため、ユニバーサルジョイント１１、１２を挟んで接合した各２本の軸要素８ａ、８ｂ、９ａ、９ｂが同じ長さだけ第１ユニット１と第２ユニッオ２から出入りして両雌ネジ部材１６、１６間の関節駆動軸８、９の全体の長さが伸縮する。このユニバーサルジョイント１１、１２をはさんでそれぞれの軸要素８ａ、８ｂ、９ａ、９ｂがそれぞれのユニット１，２から出入りすることを「関節対称駆動」という。これについてはさらに詳細に説明する。 Next, the configuration of the active coupling mechanism will be described. The active coupling mechanism includes a pair of left and right joint drive shafts 8 and 9 and one propulsion drive shaft 5. The joint drive shafts 8 and 9 control the posture of the first unit 1, and the propulsion drive shaft 5 transmits the rotational drive force to the first unit 1 as described above. The two joint drive shafts 8 and 9 and one propulsion drive shaft 5 are arranged in such a manner that the joint drive shafts 8 and 9 are arranged at a predetermined interval in parallel to the upper part. The propulsion drive shaft 5 is disposed at a position that is an inverted isosceles triangle with respect to the shafts 8 and 9.
As shown in FIG. 2, the joint drive shafts 8 and 9 are composed of two shaft elements 8a and 8b and shaft elements 9a and 9b at the central portion of the three-dimensional joints 11 and 12 (universal joint, universal joint, universal joint, etc.). A joint that bends in a three-dimensional direction, which may be a spring, which will be described below using the universal joint terminology shown in the figure) The shaft elements 8a, 8b, 9a, 9b can be bent vertically, horizontally, and obliquely. On both ends of the joint drive shafts 8 and 9, right-hand screws 15 and left-hand screws 14 in opposite directions are carved as male screws at the same pitch by a predetermined length. Further, a female screw member 16 (nut) is screwed into the right screw 15 and the left screw 14, respectively. These female screw members 16 are fixed to the bases 17 and 18 of the first unit 1 and the second unit 2 and cannot be moved. On the other hand, the joint drive shafts 8 and 9 have two shaft elements 8a and 8b that are connected by sandwiching the universal joints 11 and 12 with a right screw 15 and a left screw 14 screwed into the female screw member 16, respectively. , 9a, 9b move symmetrically (reverse direction across the universal joint) by the forward and reverse rotation of the joint drive shafts 8, 9. Therefore, each of the two shaft elements 8a, 8b, 9a, 9b joined with the universal joints 11, 12 sandwiched in and out of the first unit 1 and the second unit 2 by the same length, both female screw members 16, The entire length of the joint drive shafts 8 and 9 between 16 expands and contracts. The movement of the shaft elements 8a, 8b, 9a, and 9b with the universal joints 11 and 12 in and out of the units 1 and 2 is called "joint symmetric drive". This will be described in more detail.

第２ユニット２側の関節駆動軸８、９の一端には、関節駆動モータ６が取り付けられている。この関節駆動モータ６は正逆に回転することが可能でなければならず、しかも前記関節駆動軸８、９を回転して第１ユニット１の姿勢を操作するに十分な回転力（トルク）を備えたモータを選定しなければならない。この関節駆動モータ６は、関節駆動軸８、９の回転に伴って同じように回転してはならないが、この関節駆動モータ６の回転軸の軸芯方向にスライド可能な構造（図示せず）でなければならない。 A joint drive motor 6 is attached to one end of the joint drive shafts 8 and 9 on the second unit 2 side. The joint drive motor 6 must be able to rotate forward and backward, and has sufficient rotational force (torque) to rotate the joint drive shafts 8 and 9 to manipulate the posture of the first unit 1. The equipped motor must be selected. The joint drive motor 6 should not rotate in the same manner as the joint drive shafts 8 and 9 rotate, but is slidable in the axial direction of the rotation shaft of the joint drive motor 6 (not shown). Must.

関節駆動軸８、９に近接して、この駆動軸８、９の回転数をカウントするエンコーダ１０(EN)が備え付けられている。図２のエンコーダは関節駆動軸８，９のマーカをエンコーダ１０の検知器がカウントして関節駆動軸８、９の回転数を検知するようにしているが、このエンコーダ１０は他の構成であってもよく、例えば、関節駆動モータ６の回転数を検知するようなものであってもよい。このエンコーダ１０による関節駆動軸８、９の回転数の検出により、ユニット間（雌ネジ部材１６間）の関節駆動軸８、９の全長が判明する。 In the vicinity of the joint drive shafts 8 and 9, an encoder 10 (EN) for counting the number of rotations of the drive shafts 8 and 9 is provided. In the encoder of FIG. 2, the detector of the encoder 10 counts the markers of the joint drive shafts 8 and 9 and detects the number of rotations of the joint drive shafts 8 and 9, but this encoder 10 has another configuration. For example, the number of rotations of the joint drive motor 6 may be detected. By detecting the number of rotations of the joint drive shafts 8 and 9 by the encoder 10, the total length of the joint drive shafts 8 and 9 between the units (between the female screw members 16) is determined.

推進駆動軸５は、第１ユニット１の無限軌道４を駆動する駆動機構（図示せず）に推進駆動モータ７の回転力を伝達するための駆動軸である。この推進駆動軸５は２本の軸要素５ａ、５ｂが中央部でユニバーサルジョイント１３により連結され、このユニバーサルジョイント１３をはさんでそれぞれ上下、左右、斜め方向に曲げることができる。しかし、この推進駆動軸５は前記の関節駆動軸８、９と異なり両端部に左右の逆ネジは形成されておらず、回転は可能であるが、正逆方向の回動に伴って軸支された位置が変更することがない。即ち、前記関節駆動軸８、９のように正逆方向の回転により、第１ユニット１と第２ユニット２から軸要素５ａ、５ｂが対称に出入りすることがなく、回動は可能であるが軸芯方向に移動することなく、軸支された状態を維持する。従って、この推進駆動軸５の寸法により第１ユニット１と第２ユニット２との間隔が定まり、しかもユニバーサルジョイント１３がヒンジの役割を果たし、ユニバーサルジョイント１３を中心にして第２ユニットに対して第１ユニット１が回動することができる。 The propulsion drive shaft 5 is a drive shaft for transmitting the rotational force of the propulsion drive motor 7 to a drive mechanism (not shown) that drives the endless track 4 of the first unit 1. The propulsion drive shaft 5 has two shaft elements 5a and 5b connected to each other by a universal joint 13 at the center, and can be bent in the vertical, left and right, and diagonal directions across the universal joint 13, respectively. However, unlike the above-described joint drive shafts 8 and 9, the propulsion drive shaft 5 is not formed with left and right reverse screws at both ends, and can be rotated. However, the propulsion drive shaft 5 is supported by rotation in the forward and reverse directions. The position that has been set does not change That is, as the joint drive shafts 8 and 9 are rotated in the forward and reverse directions, the shaft elements 5a and 5b are not symmetrically moved in and out from the first unit 1 and the second unit 2, but can be rotated. The axially supported state is maintained without moving in the axial direction. Accordingly, the distance between the first unit 1 and the second unit 2 is determined by the dimensions of the propulsion drive shaft 5, and the universal joint 13 serves as a hinge. One unit 1 can rotate.

第２ユニット２側の前記推進駆動軸５の端部に推進駆動モータ７が接続している。推進駆動モータ７は前記のように第１ユニット１の無限軌道４を駆動するに十分な回転力（トルク）を出力できるものを選択しなければならない。この推進駆動モータ７は第２ユニット２の基台１８に固定されており、関節駆動モータ６のように、推進駆動モータ７の回転軸の軸芯方向にスライドしながら回転することはない。従って、前記のように推進駆動軸５も軸芯方向に動くことはない。第１ユニット１側の推進駆動軸５の端部は軸受（図示せず）に軸支されている。この軸支の構成も推進駆動軸５が、軸芯方向に移動することが出来ないように、固定されている。 A propulsion drive motor 7 is connected to the end of the propulsion drive shaft 5 on the second unit 2 side. As described above, the propulsion drive motor 7 must be selected to output a rotational force (torque) sufficient to drive the endless track 4 of the first unit 1. The propulsion drive motor 7 is fixed to the base 18 of the second unit 2 and does not rotate while sliding in the axial direction of the rotation shaft of the propulsion drive motor 7 unlike the joint drive motor 6. Therefore, the propulsion drive shaft 5 does not move in the axial direction as described above. The end of the propulsion drive shaft 5 on the first unit 1 side is pivotally supported by a bearing (not shown). The structure of this shaft support is also fixed so that the propulsion drive shaft 5 cannot move in the axial direction.

次に、前記構成の作動原理について説明する。
図３、４は本件発明の能動連結機構の作動原理を説明するための説明図である。この図面は第１ユニットを円筒状に簡略化して描いている。これらの図において、前記の説明と同様の部材は同一の符号を付してある。また、第１ユニットが回動する基準軸は図１に示す、Ｙ軸、Ｘ軸、Ｚ軸として説明する。 Next, the operation principle of the above configuration will be described.
3 and 4 are explanatory views for explaining the operating principle of the active coupling mechanism of the present invention. In this drawing, the first unit is drawn in a simplified cylindrical shape. In these drawings, the same members as those described above are denoted by the same reference numerals. The reference axis on which the first unit rotates will be described as the Y axis, the X axis, and the Z axis shown in FIG.

本発明の能動連結機構は、第１ユニット１が瓦礫等の中を探査していて、その姿勢等が不明であっても、「順運動学関係」から、関節駆動軸８、９の変位寸法量ｄ_ｌ、ｄ_ｒ（数式上、関節駆動軸変数と言うことがある）及びそれから導きだされる関節角θ_１、θ_２から第１ユニット１の位置、姿勢を解析することが可能となる。また、「逆運動学関係」から、第１ユニット１の目標位置の条件を設定すると、それを満たす関節角θ_１、θ_２及び関節駆動軸８、９の変位寸法量ｄ_ｌ、ｄ_ｒを求めて第１ユニット１を所望の姿勢となるように操作することができる。以下、これらの点について説明する。
Ｉ）第１ユニットの先端位置姿勢を求める。（順運動学関係）この順運動学関係は、前記のように第１ユニット１の先端位置姿勢が不明であっても、関節駆動軸８、９の変位寸法量ｄ_ｌ、ｄ_ｒ（左右関節駆動軸変数）から、関節角θ_１、θ_２を求め、さらに、第１ユニットのユニット先端位置姿勢を求めるものである。ユニット先端位置姿勢とは、第１ユニットの特定の位置（例えば、ＣＣＤカメラのレンズの位置）の座標軸の距離及び角度から第１ユニットの位置姿勢を表すものである。 In the active coupling mechanism of the present invention, even when the first unit 1 is exploring the debris and the like and the posture is unknown, the displacement dimension of the joint drive shafts 8 and 9 is determined from the “forward kinematics relationship”. It is possible to analyze the position and posture of the first unit 1 from the amounts d _l and _dr (sometimes referred to as joint drive axis variables in the mathematical formula) and the joint angles θ ₁ and θ ₂ derived therefrom. . Also, from the "inverse kinematics relationships", setting the conditions for the target position of the first unit 1, the joint angle theta ₁ to fill _it, the displacement size of theta ₂ and joint drive shaft 8, 9 d _l, the d _r Thus, the first unit 1 can be operated to have a desired posture. Hereinafter, these points will be described.
I) Obtain the tip position and orientation of the first unit. (Forward kinematics relationship) This forward kinematics relationships, the even tip position and orientation of the first unit 1 is not known as the displacement dimension amount d _l of joint drive shaft 8, _9, d r _(right and left joints The joint angles θ ₁ and θ ₂ are obtained from the drive shaft variables), and further the unit tip position / posture of the first unit is obtained. The unit tip position / posture represents the position / posture of the first unit from the distance and angle of the coordinate axis of a specific position of the first unit (for example, the position of the lens of the CCD camera).

前記のｄ_ｌは、左側（left）の関節駆動軸８の変位寸法量、ｄ_ｒは、右側（right）の関節駆動軸９の変位寸法量を示している。さらに変位寸法量ｄ_ｌ、ｄ_ｒを図１、図２を用いて具体的に説明する。変位寸法量ｄ_ｌ、ｄ_ｒは、折れ曲がった状態を含めて、両雌ネジ部材１６，１６間の関節駆動軸８、９の軸要素８ａ，８ｂ，９ａ，９ｂの長さ（もしくは下記基準姿勢における基準寸法に対する変化量）であるが、第１ユニット１と、第２ユニット２間の関節駆動軸８，９の軸要素８ａ，８ｂ，９ａ，９ｂの長さ（もしくは下記基準姿勢における基準寸法に対する変化量）と言い表すこともできる。この変位寸法量ｄ_ｌ、ｄ_ｒは、前記の説明のとおり、関節駆動モータ６、６により、関節駆動軸８、９を正逆方向に回転することにより、両ユニット間の全長（以下、単に寸法という）は変化する。前記のように、図２の雌ネジ部材１６，１６に螺合している関節駆動軸８，９には右ネジ１５と左ネジ１４が刻まれているので、関節駆動モータ６、６の回転により、ユニバーサルジョイント１１，１２を挟んで連結されたそれぞれの軸要素８ａ，８ｂ，９ａ，９ｂは、同寸法だけ対称（軸要素がそれぞれ逆方向）に両ユニット１，２から出入りする。第１ユニット１と第２ユニット２が、図３において、第１ユニットを点線で示すように一直線状の姿勢（以下、基準姿勢という）の場合の両ユニット１，２間の関節駆動軸８，９の寸法を基準寸法とすると、関節駆動モータ６，６の例えば正回転により軸要素８ａ，８ｂ，９ａ，９ｂがそれぞれのユニット１，２に退いていく（中に入る状態）とすると、両ユニット１，２間の関節駆動軸８，９の寸法は、前記基準寸法より短くなる。逆に、関節駆動モータ６，６を逆回転すると、軸要素８ａ，８ｂ，９ａ，９ｂはそれぞれのユニット１，２から同じ寸法だけ出てくる（中から出てくる状態）ので、両ユニット１，２間の関節駆動軸８，９の寸法は前記の基準寸法より長くなる。この両ユニット１，２間の関節駆動軸８，９の軸要素８ａ，８ｂ，９ａ，９ｂの伸縮する寸法を変位寸法量ｄ_ｌ、ｄ_ｒと言う。この変位寸法量ｄ_ｌ、ｄ_ｒは、第１ユニットの先端位置姿勢を求める際に、数式を解析する場合は左右の関節駆動軸の左右関節駆動軸変数と言う。
この関節駆動軸８，９の左右関節駆動軸変数ｄ_ｌ、ｄ_ｒから、後述する関数関係を解析することにより、関節角θ_１、θ_２を求めることができる。関節角θ_１は、図３に示すように、第１ユニット１が、上下方向に回転した場合の回転角である。より正確には、推進駆動軸５のユニバーサルジョイント１３からＹ軸方向に垂直に伸びる軸線２０（仮想Ｙ軸基準線）が、変位寸法量ｄ_ｌ、ｄ_ｒの変化に伴って、前記ユニバーサルジョイント１３の位置におけるＺ軸線を中心として回動した場合の回転角である。図３は、第１ユニット１がユニバーサルジョイント１３のＺ軸線を中心にθ_１だけ回転して上方に持上げられた状態を示す正面図である。関節角θ_２は、変位寸法量ｄ_ｌ、ｄ_ｒの変化に伴って、第１ユニット１が、仮想Ｙ軸基準線２０を中心として、左右方向に回動した場合の回転角である。図４の平面図は、図３の正面図の状態から第１ユニット１がさらに仮想Ｙ軸基準線２０を中心にしてθ_２だけ左方向に回転した状態を示している。 It said d _l, the displacement size of joint drive shaft 8 on the left side (left), d _r indicates the displacement size of joint drive shaft 9 of the right (. Right). Further, the displacement dimension amounts d _l and _dr will be specifically described with reference to FIGS. Displacement dimension amount _d l, _{d r} is bent state, including the shaft element 8a of the joint drive shafts 8 and 9 between the two female screw members 16, 16, 8b, 9a, 9b of the length (or below the reference attitude The amount of change of the shaft elements 8a, 8b, 9a and 9b of the joint drive shafts 8 and 9 between the first unit 1 and the second unit 2 (or the reference dimensions in the following reference posture). The amount of change with respect to The displacement dimensions weight d _l, d _r, as in the above description, the joint drive motor 6,6, by rotating the joint drive shaft 8, 9 in the forward and reverse directions, the overall length between the two units (hereinafter, simply Dimensions) will change. As described above, the right and left screws 15 and 14 are engraved on the joint drive shafts 8 and 9 screwed into the female screw members 16 and 16 in FIG. Thus, the respective shaft elements 8a, 8b, 9a, 9b connected with the universal joints 11 and 12 interposed therebetween enter and exit from both units 1 and 2 symmetrically by the same dimension (the shaft elements are respectively in opposite directions). The first unit 1 and the second unit 2 in FIG. 3 have joint drive shafts 8 between the units 1 and 2 when the first unit is in a straight posture (hereinafter referred to as a reference posture) as indicated by a dotted line in FIG. Assuming that the dimension 9 is the reference dimension, if the shaft elements 8a, 8b, 9a, 9b are retracted to the respective units 1, 2 by the forward rotation of the joint drive motors 6, 6, for example, The dimensions of the joint drive shafts 8 and 9 between the units 1 and 2 are shorter than the reference dimensions. On the contrary, when the joint drive motors 6 and 6 are rotated in the reverse direction, the shaft elements 8a, 8b, 9a and 9b come out of the same dimensions from the respective units 1 and 2 (in a state of coming out from the inside). , 2 are longer than the reference dimensions described above. Longitudinal element 8a of the joint drive shaft 8, 9 between the two units 1 and 2, referred 8b, 9a, weight displacement dimension stretching dimension of 9b _d l, and _{d r.} These displacement dimension amounts d _l and _dr are referred to as the left and right joint drive axis variables of the left and right joint drive axes when analyzing the mathematical expression when obtaining the tip position and orientation of the first unit.
The lateral joint drive shaft variable d _l of the joint drive shafts 8 and _9, the d _r, by analyzing the functional relationship to be described later, it is possible to determine the joint angle theta _1, theta _2. As shown in FIG. 3, the joint angle θ ₁ is a rotation angle when the first unit 1 rotates in the vertical direction. More precisely, the axis 20 extending perpendicularly from the universal joint 13 of the propulsion drive shaft 5 in the Y-axis direction (virtual Y-axis reference lines), the displacement dimension amount d _l, with a change in d _r, the universal joint 13 Is a rotation angle when rotated about the Z-axis at the position. FIG. 3 is a front view showing a state where the first unit 1 is rotated upward by θ ₁ around the Z axis of the universal joint 13 and lifted upward. The joint angle θ ₂ is a rotation angle when the first unit 1 rotates in the left-right direction around the virtual Y-axis reference line 20 as the displacement dimension amounts d ₁ and _dr change. The plan view of FIG. 4 shows a state in which the first unit 1 is further rotated leftward by θ ₂ around the virtual Y-axis reference line 20 from the state of the front view of FIG.

次に、関節駆動軸の変位寸法量ｄ_ｌ、ｄ_ｒと、関節角θ_１、θ_２との関係について説明する。
１）関節駆動軸要素８ａ，８ｂの変位寸法量ｄ_ｌと、同９ａ，９ｂの変位寸法量ｄ_ｒとが同じだけ基準寸法より短い状態。 Then, the amount displacement dimension of the joint drive shaft _d l, and _{d r,} joint angle theta _1, the relationship between theta ₂ will be described.
1) joint drive shaft element 8a, a displacement dimension amount _{d l} of 8b, the 9a, 9b displacement dimension amount _{d r} and is shorter than that reference dimension only same.

第１ユニット１が、基準姿勢の状態からユニバーサルジョイント１３のＺ軸線を中心に上方に曲がる。変位寸法量ｄ_ｌ、ｄ_ｒの変化量が大きくなると、関節角θ_１は大きくなり、変位寸法量ｄ_ｌ、ｄ_ｒの変化量が小さくなると、関節角θ_１は小さくなる。
２）関節駆動軸要素８ａ，８ｂの変位寸法量ｄ_ｌと、同９ａ，９ｂが変位寸法量ｄ_ｒとが同じだけ基準寸法より長い状態。 The first unit 1 bends upward around the Z axis of the universal joint 13 from the reference posture. Displacement dimension amount d _l, the amount of change d _r increases, the joint angle theta ₁ is increased, the displacement dimension amount d _l, the amount of change d _r becomes smaller, the joint angle theta ₁ is reduced.
2) joint drive shaft element 8a, a displacement dimension amount _{d l} of 8b, the 9a, 9b is longer than that by the reference dimension and displacement dimension amount _{d r} are the same.

第１ユニット１が、基準姿勢の状態からユニバーサルジョイント１３のＺ軸を中心に下方に曲がる。変位寸法量ｄ_ｌ、ｄ_ｒの変化量が大きくなると、関節角−θ_１の絶対値は大きくなり、変位寸法量ｄ_ｌ、ｄ_ｒの変化量が小さくなると、関節角−θ_１の絶対値は小さくなる。
３）基準寸法より、左側関節駆動軸要素８a,８ｂの変位寸法量ｄ_ｌが短く、右側関節駆動軸要素９ａ，９ｂの変位寸法量ｄ_ｒが長い状態。 The first unit 1 bends downward about the Z axis of the universal joint 13 from the reference posture. Displacement dimension amount d _l, the amount of change d _r increases, the absolute value of the joint angle - [theta] ₁ increases, the displacement dimension amount d _l, the amount of change d _r becomes smaller, the absolute value of the joint angle - [theta] ₁ Becomes smaller.
3) than the reference dimension, the left joint drive shaft element 8a, short displacement dimension amount d _l of 8b, the right joint drive shaft element 9a, the displacement dimension amount d _r of 9b long state.

第１ユニット１が、基準姿勢の状態から仮想Ｙ軸基準線２０を中心に左に曲がる。変位寸法量ｄ_ｌ、ｄ_ｒの変化量が大きくなると関節角θ_２は大きくなり、変位寸法量ｄ_ｌ、ｄ_ｒの変化量が小さくなると、関節角θ_２は小さくなる。
４）基準寸法より、左側関節駆動軸要素８ａ，８ｂの変位寸法量ｄ_ｌが長く、右側関節駆動軸要素９ａ，９ｂの変位寸法量ｄ_ｒが短い状態。 The first unit 1 turns left about the virtual Y-axis reference line 20 from the reference posture state. Displacement dimension amount d _l, the amount of change d _r is greater joint angle theta ₂ is increased, the displacement dimension amount d _l, the amount of change d _r becomes smaller, the joint angle theta ₂ is reduced.
4) than the reference dimension, the left joint drive shaft element 8a, long displacement dimension amount _{d l} of 8b, the right joint drive shaft element 9a, the displacement dimension amount _{d r} of 9b short state.

第１ユニット１が、基準姿勢の状態から仮想Ｙ軸基準線２０を中心に右に曲がる。変位寸法量ｄ_ｌ、ｄ_ｒの変化量が小さくなると関節角−θ_２の絶対値は小さくなり、変位寸法量ｄ_ｌ、ｄ_ｒの変化量が大きくなると、関節角−θ_２の絶対値は大きくなる。 The first unit 1 turns to the right around the virtual Y-axis reference line 20 from the reference posture state. Displacement dimension amount d _l, the absolute value of d _r of variation decreases as the joint angle - [theta] ₂ is reduced, the displacement dimension amount d _l, the amount of change d _r increases, the absolute value of the joint angle - [theta] ₂ is growing.

以上の１）２）３）４）の基本姿態をさらに組み合わせた、１）＋３）、１）＋４）、２）＋３）、２）＋４）の姿勢ができる。この姿勢は第１ユニット１がＺ軸線を中心に上下方向に曲がった状態から、さらに、左右の何れかの方向に回転して捻れた状態の組み合わせである。 1) +3), 1) +4), 2) +3), and 2) +4) can be combined by further combining the basic forms of 1), 2), 3), and 4). This posture is a combination of a state in which the first unit 1 is bent in the vertical direction around the Z-axis, and is further twisted by rotating in either the left or right direction.

上記の変位寸法量ｄ_ｌ、ｄ_ｒと関節角θ_１、θ_２とは幾何学的関係から次のような関数関係となる。
(ｄ_ｌ、ｄ_ｒ)＝ｇ（θ_１、θ_２）式Ｉ
この式Ｉの逆関数は次の式IIのようになり、ｄ_ｌ、ｄ_ｒより関節角θ_１、θ_２を求める際に用いる順運動学関係である。 The displacement dimensions d ₁ and _dr and the joint angles θ ₁ and θ ₂ have the following functional relationship from a geometric relationship.
(d ₁ , d _r ) = g (θ ₁ , θ ₂ ) Formula I
Inverse function of the formula I is given by the following formula II, d _l, the joint angle theta ₁ than d _{_r,} a forward kinematics relationships used to obtain the theta _2.

(θ_１、θ_２)＝ｇ^-１(ｄ_ｌ、ｄ_ｒ) 式II
上記の式Ｉ、IIのｇは関数を表し、そのパラメータ値は、能動連結機構の種々の寸法等の要素によって定まる。例えば、推進駆動軸５と関節駆動軸８，９の寸法及び関節駆動軸８，９と推進駆動軸５との距離関係等から導き出されるものである。（ｄ_ｌ、ｄ_ｒ）は前記のように関節駆動モータ６の回転数をエンコーダ１０によりカウントすることにより判明する。左右関節駆動軸変数（ｄ_ｌ、ｄ_ｒ）よりｇの逆関数（式II）から関節角θ_１、θ_２を求めることができる。 (θ ₁ , θ ₂ ) = g ⁻¹ (d _l , d _r ) Formula II
In the above formulas I and II, g represents a function, and the parameter value is determined by factors such as various dimensions of the active coupling mechanism. For example, it is derived from the dimensions of the propulsion drive shaft 5 and the joint drive shafts 8, 9 and the distance relationship between the joint drive shafts 8, 9 and the propulsion drive shaft 5. (D ₁ , d _r ) is determined by counting the number of rotations of the joint drive motor 6 by the encoder 10 as described above. The joint angles θ ₁ and θ ₂ can be obtained from the inverse function (formula II) of g from the left and right joint drive axis variables (d ₁ , _dr ).

また、能動連結機構を表す数学モデルとして関節座標系（ｘ_i,ｙ_i,ｚ_i）と関節動作を表す関節角θ_１、θ_２とを適正に設定することにより、基準座標系（ｘ_R,ｙ_R,ｚ_R）やユニット先端座標系（ｘ_E,ｙ_E,ｚ_E）を含めた座標系間の同時変換関係を表す行列
^ｉ-１Ｔ_i＝^ｉ-１Ｔ_i(θ_i)
の各要素を用いることにより、次のようなθ_１、θ_２から第１ユニット先端位置姿勢までの順運動学関係を求めることができる。 Further, by appropriately setting the joint coordinate system (x _i , y _i , z _i ) and the joint angles θ ₁ and θ ₂ representing the joint motion as a mathematical model representing the active coupling mechanism, the reference coordinate system (x _R , y _R , z _R ) and a matrix representing the simultaneous conversion relationship between coordinate systems including the unit tip coordinate system (x _E , y _E , z _E )
^i-1 T _i = ^i-1 T _i (θ _i )
By using these elements, the following forward kinematic relationship from θ ₁ , θ ₂ to the first unit tip position / posture can be obtained.

（ｘ_E,ｙ_E,ｚ_E,θ_E,φ_E,ψ_E）＝ｆ(θ_１、θ_２) 式III
上記式IIIの（ｘ_E,ｙ_E,ｚ_E）は、第１ユニット１の先端位置(Eed)における先端座標系の基準座標系に対する距離を示し、（θ_E,φ_E,ψ_E）は、先端座標系の基準座標系に対する角度を示している。この距離（ｘ_E,ｙ_E,ｚ_E）と角度（θ_E,φ_E,ψ_E）により第１ユニットの先端位置姿勢を表している。 (X _E , y _E , z _E , θ _E , φ _E , ψ _E ) = f (θ ₁ , θ ₂ ) Formula III
(X _E , y _E , z _E ) in the above formula III indicates the distance from the reference coordinate system of the tip coordinate system at the tip position (Eed) of the first unit 1, and (θ _E , φ _E , ψ _E ) is The angle of the tip coordinate system with respect to the reference coordinate system is shown. The distance (x _E , y _E , z _E ) and the angle (θ _E , φ _E , ψ _E ) represent the tip position and orientation of the first unit.

また、式IIIのｆは関数（function）を示し、ユニット１、２や能動連結機構の寸法等によって定まるものである。
以上のような式Ｉ、式II、式IIIを用いて、左右関節駆動軸変数（関節駆動軸の変位寸法量）ｄ_ｌ、ｄ_ｒから、現在の第１ユニットの先端位置姿勢を求めることができる。 Further, f in the formula III indicates a function, which is determined by the dimensions of the units 1 and 2 and the active coupling mechanism.
Formula I, Formula II as described above, by using Equation III, the left and right joint drive shaft variables (weight displacement dimension of the joint drive shaft) d _l, the d _r, be determined tip position and orientation of the current of the first unit it can.

次に、第１ユニット１の現在の先端位置姿勢をパーソナルコンピュータにより求める場合を図５のフローチャートと、図７のブロック図を用いて説明する。
図７のＩ/Ｆ，ＣＰＵ，ＲＡＭ，ＲＯＭ，ＬＣＤ（液晶ディスプレイ）等はパーソナルコンピュータを示しているが、制御用の特別なマイクロコンピュータを用いたものであってもよい。図７において、６、６は関節駆動モータ、７は推進駆動モータ、１０，１０はエンコーダ（ＥＮ）を示している。 Next, the case where the current tip position / posture of the first unit 1 is obtained by a personal computer will be described with reference to the flowchart of FIG. 5 and the block diagram of FIG.
Although the I / F, CPU, RAM, ROM, LCD (liquid crystal display), etc. in FIG. 7 are personal computers, they may be those using a special microcomputer for control. In FIG. 7, 6 and 6 are joint drive motors, 7 is a propulsion drive motor, and 10 and 10 are encoders (EN).

ステップ１；第１ユニット１の先端位置姿勢のｎ周期目の検出のスタートを示している。キーボードからの指令により、ＲＯＭ内に格納されているプログラムにより先端位置姿勢の検出を開始する。 Step 1 shows the start of detection of the tip position / posture of the first unit 1 in the nth cycle. In response to a command from the keyboard, detection of the tip position and orientation is started by a program stored in the ROM.

ステップ２；左側の関節駆動軸８に設けられているエンコーダ１０(L)から左側関節駆動軸８の変位寸法量（ｄ_ｌ）を検出する。即ち、左側のエンコーダ１０（L）の正逆方向の回転数から関節駆動軸８の現在の寸法を割り出し変位寸法量（ｄ_ｌ）を検出する。 Step 2: The displacement dimension amount (d _l ) of the left joint drive shaft 8 is detected from the encoder 10 (L) provided on the left joint drive shaft 8. That is, the current dimension of the joint drive shaft 8 is determined from the rotational speed of the left encoder 10 (L) in the forward and reverse directions, and the displacement dimension amount (d _l ) is detected.

ステップ３；右側の関節駆動軸９に設けられているエンコーダ１０(R)から右側関節駆動軸９の変位寸法量（ｄ_ｒ）を検出する。即ち、右側のエンコーダ１０（R）の正逆方向の回転数から関節駆動軸９の現在の寸法を割り出し変位寸法量（ｄ_ｒ）を検出する。 Step 3: The displacement dimension amount (d _r ) of the right joint drive shaft 9 is detected from the encoder 10 (R) provided on the right joint drive shaft 9. That is, the current dimension of the joint drive shaft 9 is determined from the number of rotations of the right encoder 10 (R) in the forward and reverse directions, and the displacement dimension (d _r ) is detected.

ステップ４、５；前記ステップ２で検出した変位寸法量（ｄ_ｌ）と、前記ステップ３で検出した変位寸法量（ｄ_ｒ）とからＺ軸回りの関節角θ_１と、仮想Ｙ軸基準線回りの関節角θ_２とを算出する。この関節角θ_１θ_２を算出する式IIの関数関係は前記の通りである。 Steps 4 and 5: The joint angle θ ₁ around the Z-axis and the virtual Y-axis reference line from the displacement dimension (d ₁ ) detected in Step 2 and the displacement dimension (d _r ) detected in Step 3 The surrounding joint angle θ ₂ is calculated. The functional relationship of Formula II for calculating the joint angle θ ₁ θ ₂ is as described above.

ステップ６；前記ステップ４、５で求めた関節角θ_１θ_２から第１ユニット１の先端位置姿勢（ｘ_E,ｙ_E,ｚ_E,θ_E,φ_E,ψ_E）を算出する。この第１ユニット１の先端位置姿勢は式IIIから求めることができることは、前記の通りである。 Step 6: The tip position / posture (x _E , y _E , z _E , θ _E , φ _E , ψ _E ) of the first unit 1 is calculated from the joint angle θ ₁ θ ₂ obtained in steps 4 and 5 above. As described above, the tip position and orientation of the first unit 1 can be obtained from Formula III.

ステップ７；前記のステップ６で求めた結果をＬＣＤに表示する。第１ユニット１の先端位置姿勢（ｘ_E,ｙ_E,ｚ_E,θ_E,φ_E,ψ_E）を出力して、第１ユニット１の先端位置姿勢を画像としてＬＣＤに表示する。 Step 7: The result obtained in Step 6 is displayed on the LCD. The tip position / posture (x _E , y _E , z _E , θ _E , φ _E , ψ _E ) of the first unit 1 is output, and the tip position / posture of the first unit 1 is displayed on the LCD as an image.

ステップ８；ステップ７により求めた第１ユニット１の先端位置姿勢からｎ＋１周期目の検出に移行する。ｎ＋１周期目以降も同様にして刻々変化する第１ユニット１の先端位置姿勢を算出し、ＬＣＤに表示することができる。
II）第１ユニット先端目標位置実現のための変位寸法量を求める。（逆運動学関係）
逆運動学関係は、第１ユニット１の先端位置姿勢から関節駆動軸８，９の変位寸法量ｄ_ｌ、ｄ_ｒへの関係を与えるものであり、操縦者が所望する第１ユニット１の先端位置姿勢を実現するために必要な関節駆動軸８，９の変位寸法量ｄ_ｌ、ｄ_ｒを求めるのに用いる。即ち、第１ユニット１を目標とする位置姿勢にするために左右の関節駆動軸８，９の寸法（ｄ_ｌ、ｄ_ｒ）をどれだけにすればよいかを解析するものである。そのためには、第１ユニットの先端位置姿勢の実現に必要な回転角θ_ｄ１、θ_ｄ２を求め、さらに必要な関節駆動軸８，９の変位寸法量（左右関節駆動軸変数）ｄ_ｄｌ、ｄ_ｄｒを求める逆運動学関係を解析的に解くことにより、第１ユニット１の先端目標位置姿勢を実現する。 Step 8: Shift to detection of the (n + 1) -th cycle from the tip position and orientation of the first unit 1 obtained in Step 7. Similarly, the tip position / posture of the first unit 1 that changes momentarily can be calculated and displayed on the LCD after the (n + 1) th cycle.
II) Obtain the displacement dimension for realizing the first unit tip target position. (Reverse kinematics)
Inverse kinematics relationships, the displacement dimension amount d _l of joint drive shaft 8, 9 from the tip position and orientation of the first unit _1, which gives the relationship between the d _r, the tip of the first unit 1 operator desires displacement dimension amount d _l of the joint drive shaft 8, 9 required to achieve the position and _orientation, is used to determine the d _r. That is, it is analyzed how much the dimensions (d ₁ , d _r ) of the left and right joint drive shafts 8 and 9 should be set in order to make the first unit 1 a target position and orientation. For this purpose, the rotation angles θ _d1 and θ _d2 necessary for realizing the tip position and orientation of the first unit are obtained, and further necessary displacement dimension amounts of the joint drive shafts 8 and 9 (left and right joint drive shaft variables) d _dl and d The tip target position / posture of the first unit 1 is realized by analytically solving the inverse kinematic relationship for _{obtaining dr} .

前記の順運動学関係の式IIIの逆関係である逆運動学関係は次の通り表されるとする。
(θ_１、θ_２)＝ｆ^-１(ｘ_E,ｙ_E,ｚ_E,θ_E,φ_E,ψ_E) 式Ｖ
いま、第１ユニット１の先端目標位置姿勢を次のように与えられるとする。 It is assumed that the inverse kinematic relationship that is the inverse relationship of Formula III of the forward kinematic relationship is expressed as follows.
(θ ₁ , θ ₂ ) = f ⁻¹ (x _E , y _E , z _E , θ _E , φ _E , ψ _E ) Formula V
Now, it is assumed that the tip target position / posture of the first unit 1 is given as follows.

（ｘ_E,ｙ_E,ｚ_E,θ_E,φ_E,ψ_E）_ｄ式VI
この式VIに逆運動学関係式Ｖを当てはめると、目標回転角θ_ｄ１、θ_ｄ２が求められる。 (X _E , y _E , z _E , θ _E , φ _E , ψ _E ) _d formula VI
When the inverse kinematic relational expression V is applied to this expression VI, the target rotation angles θ _d1 and θ _d2 are obtained.

(θ_ｄ１、θ_ｄ２)＝ｆ^-１（ｘ_E,ｙ_E,ｚ_E,θ_E,φ_E,ψ_E）_ｄ
さらに、回転角θ_ｄ１、θ_ｄ２から関数式Ｉを用いて左右関節駆動軸変数（関節駆動軸の変位寸法量）ｄ_ｄｌ、ｄ_ｄrを求めることができる。 (θ _d1 , θ _d2 ) = f ⁻¹ (x _E , y _E , z _E , θ _E , φ _E , ψ _E ) _d
Furthermore, the left and right joint drive axis variables (displacement dimension amounts of the joint drive axes) d _dl and d _dr can be obtained from the rotation angles θ _d1 and θ _d2 using the function formula I.

(ｄ_ｄl、ｄ_ｄr)＝ｇ（θ_ｄ1、θ_ｄ2）
このようにして求めた関節駆動軸の変位寸法量ｄ_ｄl、ｄ_ｄrに基づいて関節駆動モータ６を回動して回転駆動軸８，９の両ユニット１，２間の寸法をこの変位寸法量ｄ_ｄl、ｄ_ｄrに適合するように制御すれば、第１ユニット１の先端位置姿勢を実現することができる。 (d _dl , d _dr ) = g (θ _d1 , θ _d2 )
The joint drive motor 6 is rotated based on the displacement dimension amounts d _dl and d _dr of the joint drive shaft thus obtained, and the dimension between the units 1 and 2 of the rotary drive shafts 8 and 9 is determined as the displacement dimension amount. If control is performed so as to conform to d _dl and d _dr , the tip position and orientation of the first unit 1 can be realized.

次に、前記の説明のうち、第１ユニットの先端位置姿勢を実現するための制御方法について、図６のフローチャートと、図７のブロック図を用いて説明する。図７のＩ/Ｆ，ＣＰＵ，ＲＡＭ，ＲＯＭ，ＬＣＤ等はパーソナルコンピュータを示しているが、制御用の特別なコントローラであってもよい。 Next, a control method for realizing the tip position / posture of the first unit will be described with reference to the flowchart of FIG. 6 and the block diagram of FIG. Although the I / F, CPU, RAM, ROM, LCD, etc. in FIG. 7 represent personal computers, they may be special controllers for control.

先ず、前記のように、逆運動学関係により第１ユニット１の先端位置姿勢（ｘ_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}とを足し合わせて右側関節駆動軸９の目標変位寸法量ｄ_ｄｒを得（ステップ５）、それらをサーボ制御により実現する。 First, as described above, the first unit necessary for realizing the tip position / posture (x _E , y _E , z _E , θ _E , φ _E , ψ _E ) _d of the first unit 1 due to the inverse kinematic relationship. 1 of the rotation angle theta _d1, derive theta _d2, the rotation angle theta _d1 of the first unit 1 is rotated up and down about axis Z axis of the universal joint 13, first as a center axis the imaginary Y axis reference lines 20 The unit 1 is disassembled into a rotation angle θ _d2 that turns left and right. An operation command with the Z-axis line of the resolved rotation angle θ _d1 as the central axis and an operation command with the virtual Y-axis reference line 20 of the rotation angle θ _d2 as the central axis are issued. (Step 1)
Next, the joint drive displacement dimension d _{dl 1 of} the left joint drive shaft 8 and the joint drive displacement dimension d _{dr 1 of the} right joint drive shaft 9 are decomposed from the rotation angle θ _d1 disassembled in (Step 1). (Step 2)
Further, the rotation angle θ _d2 decomposed in (Step 1) is decomposed into a joint drive displacement dimension d _{dl 2 of} the left joint drive shaft 8 and a joint drive displacement dimension d _{dr 2 of the} right joint drive shaft 9. (Step 3)
Next, using the joint drive displacement dimension d _{dl 1} , joint drive displacement dimension d _dr1 , joint drive displacement dimension d _{dl 2} , and joint drive displacement dimension d _{dr 2} disassembled in Step 2 and Step 3, d The target displacement dimension d _dl of the left joint drive shaft 8 is obtained by adding _{dl 1} and d _{dl 2} (step 4), and the target displacement dimension of the right joint drive shaft 9 is added by adding d _dr1 and d _{dr 2.} The quantities d _dr are obtained (step 5) and realized by servo control.

図６の左端のシーケンスは、左側関節駆動軸８の目標変位寸法量ｄ_ｄｌを実現するためのサーボ制御をデジタルコンピュータを用いて離散的に実現する場合を示している。このサーボ制御（Ｓ.Ｃ.Ｉ）はステップ４によって得られる左側関節駆動軸８の目標変位寸法量ｄ_ｄｌと、現在の左側の関節駆動軸８の変位寸法量ｄ_ｌを比較する。左側関節駆動軸８の目標変位寸法量ｄ_ｄｌと、現在の左側の関節駆動軸８の変位寸法量ｄ_ｌとの差が０の場合、即ち左側関節駆動軸８の変位寸法量ｄ_ｌを変える必要がない場合は現在のｄ_ｌを保持し次ぎの制御周期に移る。
もし、左側関節駆動軸の目標変位寸法量ｄ_ｄｌと、現在の左側の関節駆動軸８の変位寸法量ｄ_ｌ間に誤差がある場合は、その誤差を修正する制御を行う。即ち、エンコーダ１０（ＥＮ）により関節駆動軸８の回転数をカウントして、得られる現在の変位寸法量ｄ_ｌを、ステップ４で得られる左側関節駆動軸の目標変位寸法量ｄ_ｄｌと比較する。そして、その差に対してゲイン（誤差の変換・増幅率）Ｃの定めに基づき増幅し、さらにドライバによりモータ駆動入力に増幅し、左側のアクチュエータ（関節駆動モータ６）に通電し、左側関節駆動軸の目標変位寸法量ｄ_ｄｌと、現在の左側関節駆動軸の変位寸法量ｄ_ｌとの間の差が０となる方向に回転する。このようなサーボ制御により、左側関節駆動軸の目標変位寸法量ｄ_ｄｌと、現在の左側関節駆動軸の変位寸法量ｄ_ｌの誤差を０にする。
シーケンス（Ｓ.Ｃ.Ｉ）と同時に右隣りの第２の制御シーケンス（Ｓ.Ｃ.II）を同様に実施して、ステップ５で得られる右側関節駆動軸の目標変位寸法量ｄ_ｄｒと、現在の右側の関節駆動軸の変位寸法量ｄ_ｒを比較して、誤差がある場合はアクチュエータにより修正して誤差をなくすよう制御を行う。 The sequence at the left end of FIG. 6 shows a case where servo control for realizing the target displacement dimension amount d _dl of the left joint drive shaft 8 is realized discretely using a digital computer. The servo control (S.C.I) compares the target displacement dimension amount d _dl of left joint drive shaft 8 obtained in step 4, the displacement dimension amount d _l of the current left joint drive shaft 8. Changing the target displacement dimension amount d _dl of left joint drive shaft 8, when the difference between the displacement dimension amount d _l of the current left joint drive shaft 8 is zero, i.e. the displacement dimension amount d _l of the left joint drive shaft 8 If it is not necessary, the current _dl is held and the next control cycle is started.
If the target displacement dimension amount d _dl of left joint drive shaft, if there is an error between the displacement dimension amount d _l of the current left joint drive shaft 8 performs control to correct the error. That is, by counting the number of revolutions of the joint drive shaft 8 by the encoder 10 (EN), the current displacement dimension amount d _l obtained, compared with the target displacement dimension amount d _dl of left joint drive shaft obtained in Step 4 . Then, the difference is amplified based on the definition of gain (error conversion / amplification factor) C, further amplified to the motor drive input by the driver, and the left actuator (joint drive motor 6) is energized to drive the left joint. a target displacement dimension amount d _dl of the shaft, rotates in the direction difference is zero between the displacement dimension amount d _l of the current left joint drive shaft. By such servo control, the error between the target displacement dimension amount d _dl of the left joint drive shaft and the current displacement dimension amount d _l of the left joint drive shaft is set to zero.
At the same time as the sequence (S.C.I), the second control sequence (S.C.II) adjacent to the right is similarly performed, and the target displacement dimension amount d _{dr of the} right joint drive shaft obtained in step 5; comparing the displacement dimension amount d _r of the current right of joint drive shaft, if there is an error performs control so as to eliminate the error and correct the actuator.

本発明の応用例の一例として震災や災害時に建造物等の瓦礫の中を探査するための探査用装置に応用することができる。 As an example of application of the present invention, the present invention can be applied to an exploration device for exploring a debris such as a building in the event of an earthquake or disaster.

本発明の能動連結機構の斜視図である。It is a perspective view of the active connection mechanism of this invention. 本発明の能動連結機構の部分構成図である。It is a partial block diagram of the active connection mechanism of this invention. 本発明の能動連結機構の作動原理を説明するための説明図である。It is explanatory drawing for demonstrating the action | operation principle of the active connection mechanism of this invention. 本発明の能動連結機構の作動原理を説明するための説明図である。It is explanatory drawing for demonstrating the action | operation principle of the active connection mechanism of this invention. 本発明の能動連結機構の位置姿勢の検出方法を示すフローチャートである。It is a flowchart which shows the detection method of the position and orientation of the active connection mechanism of this invention. 本発明の能動連結機構の制御方法を示すフローチャートである。It is a flowchart which shows the control method of the active connection mechanism of this invention. 本発明の能動連結機構の検出部・制御部等のブロック図である。It is a block diagram of the detection part, control part, etc. of the active connection mechanism of this invention.

Explanation of symbols

１第１ユニット
２第２ユニット
３ケーブル
４無限軌道
５推進駆動軸
６関節駆動装置
７推進駆動装置
８関節駆動軸
８ａ軸要素
８ｂ軸要素
９関節駆動軸
９ａ軸要素
９ｂ軸要素
１０エンコーダ
１１ユニバーサルジョイント
１２ユニバーサルジョイント
１３ユニバーサルジョイント
１４左ネジ
１５右ネジ
１６雌ネジ部材
１７基台
１８基台
２０仮想Ｙ軸基準線 DESCRIPTION OF SYMBOLS 1 1st unit 2 2nd unit 3 Cable 4 Endless track 5 Propulsion drive shaft 6 Joint drive device 7 Propulsion drive device 8 Joint drive shaft 8a Shaft element 8b Shaft element 9 Joint drive shaft 9a Shaft element 9b Shaft element 10 Encoder 11 Universal joint 12 Universal joint 13 Universal joint 14 Left-hand thread 15 Right-hand thread 16 Female thread member 17 Base 18 Base 20 Virtual Y-axis reference line

Claims

In the active connection mechanism that connects the preceding first unit and the following second unit and transmits the driving force from the second unit to the first unit,
This active connection mechanism consists of a pair of joint drive shafts and propulsion drive shafts, and the central part of each shaft is joined by a three-dimensional joint,
The joint drive shaft is rotatably supported by forming opposite-direction screws at both ends, and each shaft element of the joint drive shaft joined by a three-dimensional joint by rotation is symmetrical from each unit. The length of the joint drive shaft between each unit is variable,
Both ends of the propulsion drive shaft are rotatably supported by each unit, the length between each unit is fixed, and the first unit is operated around a three-dimensional joint at the center. Active linkage mechanism.

The active coupling mechanism according to claim 1, wherein the pair of joint drive shafts and the propulsion drive shafts are arranged in an inverted isosceles triangle.

The center part consists of a pair of joint drive shaft and propulsion drive shaft joined by a three-dimensional joint, and connects the preceding first unit and the following second unit, and transmits the driving force from the second unit to the first unit. In the method for detecting the position and orientation of the active connection mechanism for transmission,
The size of the pair of joint drive shafts is detected, the joint angle of the three-dimensional joint of the propulsion drive shaft is obtained from the detected size of the joint drive shaft, and then the tip position / posture of the first unit is obtained. For detecting the position and orientation of the active coupling mechanism.

The center part consists of a pair of joint drive shaft and propulsion drive shaft joined by a three-dimensional joint, and connects the preceding first unit and the following second unit, and transmits the driving force from the second unit to the first unit. In the control method of the active connection mechanism for transmitting,
Determine the tip position / posture of the desired first unit, determine the joint angle of the propulsion drive shaft necessary to realize the tip position / posture, and determine the displacement dimension of the joint drive shaft from the joint angle of the three-dimensional joint of the propulsion drive shaft And controlling the size of the pair of joint drive shafts to the displacement dimension, and controlling the active coupling mechanism.