JP7000132B2 - Drilling support device - Google Patents
Drilling support device Download PDFInfo
- Publication number
- JP7000132B2 JP7000132B2 JP2017224218A JP2017224218A JP7000132B2 JP 7000132 B2 JP7000132 B2 JP 7000132B2 JP 2017224218 A JP2017224218 A JP 2017224218A JP 2017224218 A JP2017224218 A JP 2017224218A JP 7000132 B2 JP7000132 B2 JP 7000132B2
- Authority
- JP
- Japan
- Prior art keywords
- joint
- target value
- ref
- joints
- joint group
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Landscapes
- Earth Drilling (AREA)
Description
本発明は、トンネル工事に用いられる穿孔装置に適用される穿孔支援装置に関する。 The present invention relates to a drilling support device applied to a drilling device used for tunnel construction.
従来、移動台車に取り付けられたブームと、ブームに取り付けられたガイドシェルと、ガイドシェルに取り付けられ、穿孔ロッドを有する穿孔機と、を備えた穿孔装置が知られている（例えば、特許文献１参照。）。この特許文献１に記載の穿孔装置では、ブームとガイドシェルとの位置や姿勢をセンサーで測定し、測定した位置や姿勢をモニターに表示させることで、穿孔ロッドの先端部の位置や姿勢の調整を支援するようになっている。
しかしながら、特許文献１に記載の穿孔装置では、例えば、穿孔ロッドの先端部の姿勢を調整する場合、ブーム等の関節を動作させて、穿孔ロッドの姿勢を変化させると、穿孔ロッドの先端部の位置も変化するため、オペレータに高い技術が必要とされる。
Conventionally, a punching device including a boom attached to a mobile carriage, a guide shell attached to the boom, and a punching machine attached to the guide shell and having a punching rod is known (for example, Patent Document 1). reference.). In the drilling device described in
However, in the perforation device described in
本発明は、上記のような点に着目してなされたもので、穿孔ロッドの先端部の位置及び姿勢をより容易に調整可能な穿孔支援装置を提供することを目的とする。 The present invention has been made focusing on the above points, and an object of the present invention is to provide a drilling support device capable of more easily adjusting the position and posture of the tip portion of the drilling rod.
上記課題を解決するために、本発明の一態様は、（ａ）移動台車に取り付けられ、回転関節及び直動関節を含む複数の関節が設けられた支持部材と、支持部材に取り付けられ、穿孔ロッドを有する穿孔機とを備えた穿孔装置における、穿孔ロッドの先端部の位置及び姿勢の制御を支援する穿孔支援装置であって、（ｂ）支持部材の第１の構成部品と移動台車とを相対変位可能とする第１の関節、第１の構成部品と支持部材の第２の構成部品とを相対変位可能とする第２の関節、及び第２の構成部品と支持部材の第３の構成部品とを相対変位可能とする第３の関節を含む第１の関節群の動作速度の目標値を算出する第１の目標値演算部と、（ｃ）第１の目標値演算部で算出した第１の関節群の動作速度の目標値に基づき、第１の関節群それぞれの動作速度を制御する第１の関節群動作制御部と、（ｄ）支持部材に形成された関節のうちの第１の関節群以外の１または複数の関節からなる第２の関節群の動作速度の目標値を算出する第２の目標値演算部と、（ｅ）第２の目標値演算部で算出した第２の関節群の動作速度の目標値に基づき、第２の関節群それぞれの動作速度を制御する第２の関節群動作制御部と、（ｆ）オペレータからの第２の関節群の動作の指示入力を受け付ける指示入力部と、を備え、（ｇ）第２の目標値演算部は、指示入力部で受け付けた指示入力に基づき、第２の関節群の動作速度の目標値を算出し、（ｈ）第１の目標値演算部は、第２の目標値演算部で算出した第２の関節群の動作速度の目標値に基づき、第２の関節群の動作に起因して穿孔ロッドの先端部に発生することが予期される移動速度をゼロとするための、第１の関節群の動作速度の目標値を算出する穿孔支援装置であることを要旨とする。 In order to solve the above problems, one aspect of the present invention is (a) a support member attached to a mobile carriage and provided with a plurality of joints including a rotary joint and a linear motion joint, and a support member attached to the support member and perforated. A drilling support device that assists in controlling the position and orientation of the tip of a drilling rod in a drilling device equipped with a drilling machine having a rod, wherein (b) a first component of a support member and a mobile carriage. A first joint that allows relative displacement, a second joint that allows relative displacement of the first component and a second component of the support member, and a third configuration of the second component and support member. Calculated by the first target value calculation unit that calculates the target value of the operating speed of the first joint group including the third joint that allows relative displacement of the parts, and (c) the first target value calculation unit. Based on the target value of the motion speed of the first joint group, the first joint group motion control unit that controls the motion velocity of each of the first joint groups, and (d) the first of the joints formed on the support member. The second target value calculation unit for calculating the target value of the motion speed of the second joint group consisting of one or more joints other than the one joint group, and (e) the second target value calculation unit calculated by the second target value calculation unit. Based on the target value of the motion speed of the two joint groups, the second joint group motion control unit that controls the motion velocity of each of the second joint groups , and (f) the instruction of the motion of the second joint group from the operator. It is equipped with an instruction input unit that accepts input, and (g) the second target value calculation unit calculates the target value of the motion speed of the second joint group based on the instruction input received by the instruction input unit. h ) The first target value calculation unit is based on the target value of the movement speed of the second joint group calculated by the second target value calculation unit, and the tip of the drilling rod is caused by the movement of the second joint group. The gist is that it is a drilling support device that calculates a target value of the motion speed of the first joint group in order to make the movement speed expected to occur in the joint to zero.
本発明によれば、第２の関節群の動作に起因して穿孔ロッドの先端部に発生することが予期される移動速度が打ち消されるように、第１の関節群を動作できる。そのため、穿孔ロッドの先端部の位置及び姿勢をより容易に調整可能な穿孔支援装置を提供できる。 According to the present invention, the first joint group can be operated so that the movement speed expected to occur at the tip of the perforation rod due to the movement of the second joint group is canceled out. Therefore, it is possible to provide a drilling support device capable of more easily adjusting the position and posture of the tip end portion of the drilling rod.
本発明の実施形態に係る穿孔支援装置について図面を参照して説明する。
（構成）
図１に示すように、穿孔支援装置１は、切羽２に対し爆薬装填用の装薬孔３を穿孔するためのトンネル工事用作業用車両（以下、「穿孔装置４」とも呼ぶ）に搭載される。
穿孔装置４は、移動台車５と、移動台車５に取り付けられ、回転関節及び直動関節を含む複数の関節が設けられた支持部材６と、支持部材６に取り付けられ、穿孔ロッド７ａを有する穿孔機７と、支持部材６の各関節（回転関節、直動関節）それぞれを駆動させる関節駆動装置８と、を備えている。支持部材６は、移動台車５に取り付けられたブーム９と、ブーム９に取り付けられ、穿孔機７を搭載するガイドシェル１０と、を備えている。
The drilling support device according to the embodiment of the present invention will be described with reference to the drawings.
(Constitution)
As shown in FIG. 1, the
The
図２（ａ）（ｂ）に示すように、ブーム９は、移動台車５に設けられたヨーク５ａに取り付けられたペデステル９ａと、ペデステル９ａに取り付けられた角筒状のアウターブーム９ｂと、アウターブーム９ｂに挿入された角柱状のインナーブーム９ｃと、インナーブーム９ｃに取り付けられたフォーク９ｄと、を備えている。また、フォーク９ｄに取り付けられたチルトボディ９ｅと、チルトボディ９ｅに取り付けられたスイングボディ９ｆと、スイングボディ９ｆに取り付けられたロータリーボディ９ｇと、ロータリーボディ９ｇに設けられた腕部９ｈに取り付けられたガイドマウンチング９ｉと、を更に備えている。腕部９ｈは、ロータリーボディ９ｇの回転軸と直交する方向に延びて設けられている。
As shown in FIGS. 2A and 2B, the
ヨーク５ａとペデステル９ａとの間（連結部）には、鉛直方向（図２のＺ方向）に延びている軸を回転軸（以下、「第１の回転軸１１ａ」とも呼ぶ）とする回転関節（以下、「第１の関節１２ａ」とも呼ぶ）が形成されている。また、ペデステル９ａとアウターブーム９ｂとの間（連結部）には、移動台車５の左右方向（水平方向）に延びている軸を回転軸（以下、「第２の回転軸１１ｂ」とも呼ぶ）とする回転関節（以下、「第２の関節１２ｂ」とも呼ぶ）が形成されている。さらに、アウターブーム９ｂとインナーブーム９ｃとの連結部には、アウターブーム９ｂの長手方向に延びている直動軸（以下、「第３の直動軸１１ｃ」とも呼ぶ）に沿って伸縮する直動関節（以下、「第３の関節１２ｃ」とも呼ぶ）が形成されている。すなわち、第１の関節１２ａは移動台車５とペデステル９ａとを相対変位可能とし、第２の関節１２ｂはペデステル９ａとアウターブーム９ｂとを相対変位可能とし、第３の関節１２ｃはアウターブーム９ｂとインナーブーム９ｃとを相対変位可能とする。
Between the
なお、第１、第２の関節１２ａ、１２ｂを回転関節とし、第３の関節１２ｃを直動関節とする例を示したが、他の構成を採用することもできる。例えば、第２、第３の関節１２ｂ、１２ｃを回転関節とし、第１の関節１２ａを直動関節としてもよい。また、第１、第３の関節１２ａ、１２ｃを回転関節とし、第２の関節１２ｂを回転関節としてもよい。
フォーク９ｄとチルトボディ９ｅとの連結部には、第２の回転軸１１ｂと平行な移動台車５の左右方向に延びている軸を回転軸（以下、「第４の回転軸１１ｄ」とも呼ぶ）とする回転関節（以下、「第４の関節１２ｄ」とも呼ぶ）が形成されている。さらに、チルトボディ９ｅとスイングボディ９ｆとの連結部には、第４の回転軸１１ｄと直交する方向に延びている軸を回転軸（以下、「第５の回転軸１１ｅ」とも呼ぶ）とする回転関節（以下、「第５の関節１２ｅ」とも呼ぶ）が形成されている。図２（ａ）（ｂ）の例では、第５の回転軸１１ｅは、第１の回転軸１１ａ（鉛直方向に延びている軸）と平行となっている。
Although the example in which the first and
At the connecting portion between the
スイングボディ９ｆとロータリーボディ９ｇとの連結部には、第５の回転軸１１ｅと直交する方向に延びている軸を回転軸（以下、「第６の回転軸１１ｆ」とも呼ぶ）とする回転関節（以下、「第６の関節１２ｆ」とも呼ぶ）が形成されている。図２（ａ）（ｂ）の例では、第６の回転軸１１ｆは、第３の直動軸１１ｃと平行となっている。また、腕部９ｈとガイドマウンチング９ｉとの連結部には、腕部９ｈの方向（第６の回転軸１１ｆと直交する方向）に延びている軸を回転軸（以下、「第７の回転軸１１ｇ」とも呼ぶ）とする回転関節（以下、「第７の関節１２ｇ」とも呼ぶ）が形成されている。図２（ａ）（ｂ）の例では、第７の回転軸１１ｇは、第２、第４の回転軸１１ｂ、１１ｄと平行となっている。
At the connecting portion between the
ガイドシェル１０は、ガイドマウンチング９ｉに取り付けられている。ガイドマウンチング９ｉとガイドシェル１０との連結部には、第７の回転軸１１ｇと直交する方向（穿孔ロッド７ａの方向）に延びている直動軸（以下、「第８の直動軸１１ｈ」とも呼ぶ）に沿って伸縮する直動関節（以下、「第８の関節１２ｈ」とも呼ぶ）が形成されている。図２（ａ）（ｂ）の例では、第８の直動軸１１ｈは、第６の回転軸１１ｆと平行となっている。
図１に戻り、穿孔機７は、先端部に穿孔用ビットを有する穿孔ロッド７ａと、穿孔ロッド７ａの後端部に打撃を付与するドリフタ７ｂと、を備えている。穿孔ロッド７ａは、第８の直動軸１１ｈと平行な方向に延びている。そして、穿孔機７は、穿孔ロッド７ａの後端部にドリフタ７ｂで打撃を付与することで、切羽２に装薬孔３を穿孔可能となっている。
The
Returning to FIG. 1, the
関節駆動装置８は、後述するコントローラ１８からの指令に従って、第１～第８の関節１２ａ～１２ｈそれぞれを駆動（回転、伸縮）させる。関節駆動装置８としては、例えば、第１～第８の関節１２ａ～１２ｈそれぞれに設けられた油圧シリンダ等の油圧駆動装置（不図示）への油圧供給を制御して、第１～第８の関節１２ａ～１２ｈそれぞれを駆動可能な油圧制御装置を採用できる。また、関節駆動装置８は、後述する第１の回転角θ1と第５の回転角θ5とが互いに反対方向に同じだけ回転するように、第１、第５の関節１２ａ、１２ｅを駆動させ、第２の回転角θ2と第４の回転角θ4とが互いに反対方向に同じだけ回転するように、第２、第４の関節１２ｂ、１２ｄを駆動させる同調機能を有している。
これにより、穿孔装置４は、コントローラ１８からの指令に応じて、第１～第８の関節１２ａ～１２ｈの回転や伸縮を行い、穿孔ロッド７ａの先端部の位置及び姿勢を変更可能となっている。そして、穿孔ロッド７ａの先端部を切羽２の所定位置に所定姿勢で押し当てる動作と、穿孔ロッド７ａの後端部にドリフタ７ｂで打撃を付与する動作とを繰り返すことで、切羽２の所定位置に所定角度を有する複数の装薬孔３を穿孔可能となっている。
The
As a result, the
また、穿孔支援装置１は、図１に示すように、動作量検出部１３ａ、１３ｂ、１３ｃ、１３ｄ、１３ｅ、１３ｆ、１３ｇ、１３ｈと、モード切替スイッチ１４と、指示入力部１５と、計画孔記憶部１６と、モニター１７と、コントローラ１８と、を備えている。
動作量検出部１３ａは、第１の関節１２ａによるペデステル９ａの回転角度（以下、「第１の回転角θ1」とも呼ぶ）を検出する。また、動作量検出部１３ｂは、第２の関節１２ｂによるアウターブーム９ｂの回転角度（以下、「第２の回転角θ2」とも呼ぶ）を検出する。さらに、動作量検出部１３ｃは、第３の関節１２ｃによる伸縮量（以下、「第３の伸縮量Ｌ3」とも呼ぶ）を検出する。また、動作量検出部１３ｄは、第４の関節１２ｄによるチルトボディ９ｅの回転角度（以下、「第４の回転角θ4」とも呼ぶ）を検出する。
Further, as shown in FIG. 1, the
The motion
動作量検出部１３ｅは、第５の関節１２ｅによるスイングボディ９ｆの回転角度（以下、「第５の回転角θ5」とも呼ぶ）を検出する。また、動作量検出部１３fは、第６の関節１２ｆによるロータリーボディ９ｇの回転角度（以下、「第６の回転角θ6」とも呼ぶ）を検出する。さらに、動作量検出部１３ｇは、第７の関節１２ｇによるガイドマウンチング９ｉの回転角度（以下、「第７の回転角θ7」とも呼ぶ）を検出する。また、動作量検出部１３ｈは、第８の直動軸１１ｈの伸縮量（以下、「第８の伸縮量Ｌ8」とも呼ぶ）を検出する。第１、第２、第４～第７の回転角θ1、θ2、θ4～θ7、及び第３、第８の伸縮量Ｌ3、Ｌ8の正方向は、図２に矢印で示す方向とする。そして、動作量検出部１３ａ～１３ｈは、検出結果（θ1、θ2、Ｌ3、θ4～θ7、Ｌ8）をコントローラ１８に出力する。
The motion amount detection unit 13e detects the rotation angle of the
モード切替スイッチ１４は、オペレータからの動作モードの切り替えの指示入力を受け付ける。動作モードとしては、例えば、穿孔ロッド７ａの先端部の位置を維持しつつ第４、第５の回転角θ4、θ5を操作可能とする第１のモード、穿孔ロッド７ａの先端部の位置を維持しつつ第６の回転角θ6を操作可能とする第２のモード、及び穿孔ロッド７ａの先端部の位置を維持しつつ第７の回転角θ7を操作可能とする第３のモードを有している。すなわち、第１のモードではθ4、θ5への操作に対して穿孔ロッド７ａの先端部の位置を維持する補正制御を行い、第２のモードではθ6への操作に対して補正制御を行い、第３のモードではθ7への操作に対して補正制御を行う。また、穿孔ロッド７ａの先端部の位置や姿勢を自動制御する第４のモード、及び第１～第４のモードのような位置や姿勢の維持の制御を行わない、つまり、手動制御とする第５のモードを更に有している。そして、モード切替スイッチ１４は、受け付けた指示入力をコントローラ１８に出力する。
なお、第１～第３のモードの実行中は補正制御を行うθ4～θ7以外の回転角等も、入力・操作可能としてもよい。また、第１～第３のモードを同時に実行可能としてもよい。
The mode changeover switch 14 receives an instruction input for switching the operation mode from the operator. As the operation mode, for example, the position of the tip of the
It should be noted that during the execution of the first to third modes, rotation angles other than θ 4 to θ 7 for which correction control is performed may be input / operated. Further, the first to third modes may be executed at the same time.
また、第４のモードの実行中は、第２、第３のモードを実行可能としてもよい。ただし、θ1～θ5は、自動制御とバッティングするため、入力しても動かすことができない。
指示入力部１５は、オペレータからの第１～第８の関節１２ａ～１２ｈの回転や収縮の指示入力を受け付ける。そして、受け付けた指示入力をコントローラ１８に出力する。
計画孔記憶部１６は、切羽２への穿孔パターンを記憶している。穿孔パターンとしては、例えば、穿孔が計画されている装薬孔３（以下、「計画孔」とも呼ぶ）の開口位置ｘref、ｙref、ｚref、挿し角φref、ψref、及び穿孔長からなるパターンを採用できる。
モニター１７は、オペレータから視認可能な位置に配置され、コントローラ１８で生成された画像データ（切羽２の画像と計画孔の位置のマーカーの画像データ）を表示する。
Further, while the fourth mode is being executed, the second and third modes may be made executable. However, since θ 1 to θ 5 are batting with automatic control, they cannot be moved even if they are input.
The
The planning
The
コントローラ１８は、Ａ/Ｄ(Analog to Digital)変換回路、Ｄ/Ａ(Digital to Analog)変換回路、ＣＰＵ(Central Processing Unit)、ＲＯＭ(Read Only Memory)及びＲＡＭ(Random Access Memory)等から構成した集積回路を備える。ＲＯＭは、各種処理を実現するプログラムを記憶している。ＣＰＵは、ＲＯＭが記憶しているプログラムに従って、計画孔記憶部１６が記憶している穿孔パターンに基づき、切羽２の画像とともに計画孔の位置のマーカーを表示させる指令をモニター１７に出力する。これにより、オペレータは、モニター１７を視認しながら、目的のマーカー位置に穿孔ロッド７ａの先端を移動させる。
The
また、ＣＰＵは、ＲＯＭが記憶しているプログラムに従って、第１の目標値演算部１８ａ（第１の演算実行部１８ｂ及び第２の演算実行部１８ｃを含む）と、第１の関節群動作制御部１８ｄと、第２の目標値演算部１８ｅと、第２の関節群動作制御部１８ｆと、位置取得部１８ｇとを実現し、これら１８ａ～１８ｇにより、モード切替スイッチ１４が出力する指示入力に基づき、第１のモード実行処理、第２のモード実行処理、第３のモード実行処理、第４のモード実行処理、及び第５のモード実行処理のいずれかを実行する。
Further, the CPU has a first target
（第１のモード実行処理）
次に、コントローラ１８（第１、第２の目標値演算部１８ａ、１８ｅ、第１、第２の関節群動作制御部１８ｄ、１８ｆ）が実行する第１のモード実行処理について説明する。第１のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第１のモードを表している場合に、所定時間（例えば、１０［msec］）が経過するたびに実行される。
図３に示すように、まず、ステップＳ１０１では、第２の目標値演算部１８ｅは、指示入力部１５が出力する第４、第５の関節１２ｄ、１２ｅの動作の指示入力に基づき、第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/dt、dθ5ref/dtを算出する。
(First mode execution process)
Next, the first mode execution process executed by the controller 18 (first and second target
As shown in FIG. 3, first, in step S101, the second target
続いてステップＳ１０２に移行して、第１の目標値演算部１８ａは、ステップＳ１０１で算出した目標値dθ4ref/dt、dθ5ref/dtに基づき、第４、第５の関節１２ｄ、１２ｅの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtをゼロとするための、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する。具体的には、図４に示すように、目標値dθ4ref/dt、dθ5ref/dtに基づき、下記（１）式に従って、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する。その際、第６、第７の関節１２ｆ、１２ｇの角速度の目標値ｄθ6ref/dt、ｄθ7ref/dtはゼロとする。
〔d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dt〕T
=0Ｊ8(48)・〔dθ4ref/dt、dθ5ref/dt、dθ6ref/dt、dθ7ref/dt〕T
〔d0ｘ5/dt、d0ｙ5/dt、d0ｚ5/dt〕T
=〔d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dt〕T
〔dθ1ref/dt、dθ2ref/dt、dＬ3ref/dt〕T
=0Ｊ5(13)
-1・〔d0ｘ5/dt、d0ｙ5/dt、d0ｚ5/dt〕T …（１）
なお、上記（１）式の導出手順、及び行列0Ｊ8(48)、0Ｊ5(13)
-1については後述する。
Subsequently, the process proceeds to step S102, and the first target
[D 0 x 8 / dt, d 0 y 8 / dt, d 0 z 8 / dt] T
= 0 J 8 (48) · [dθ 4 ref / dt, dθ 5 ref / dt, dθ 6 ref / dt, dθ 7 ref / dt] T
[D 0 x 5 / dt, d 0 y 5 / dt, d 0 z 5 / dt] T
= [D 0 x 8 / dt, d 0 y 8 / dt, d 0 z 8 / dt] T
[Dθ 1 ref / dt, dθ 2 ref / dt, dL 3 ref / dt] T
= 0 J 5 (13) -1 · [d 0 x 5 / dt, d 0 y 5 / dt, d 0 z 5 / dt] T … (1)
The procedure for deriving the above equation (1) and the matrices 0 J 8 (48) and 0 J 5 (13) -1 will be described later.
続いてステップＳ１０３に移行して、第１の関節群動作制御部１８ｄは、ステップＳ１０２で算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dt、及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１、第２の関節１２ａ、１２ｂの角速度dθ1/dt、dθ2/dt、及び第３の関節１２ｃの伸縮速度dＬ3/dtを目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtと等しくするための指令を関節駆動装置８に出力する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第１～第３の関節１２ａ～１２ｃを駆動させ、第１～第３の関節１２ａ～１２ｃの動作速度dθ1/dt、dθ2/dt、dＬ3/dtを制御する。
Subsequently, in step S103, the first joint group
また同時に、第２の関節群動作制御部１８ｆは、ステップＳ１０１で算出した第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/dt、dθ5ref/dt、及び動作量検出部１３ｄ、１３ｅの検出結果に基づき、第４、第５の関節１２ｄ、１２ｅの角速度dθ4/dt、dθ5/dtを目標値dθ4ref/dt、dθ5ref/dtと等しくするための指令を関節駆動装置８に出力した後、この演算処理、つまり、第１のモード実行処理を終了する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第４、第５の関節１２ｄ、１２ｅを駆動させ、第４、第５の関節１２ｄ、１２ｅの動作速度dθ4/dt、dθ5/dtを制御する。
これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１～第５の関節１２ａ～１２ｅそれぞれを駆動させることで、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、穿孔ロッド７ａの姿勢を操作可能とする。
At the same time, the second joint group
As a result, the
ここで、穿孔装置４は、穿孔ロッド７ａの先端部の位置を決めるために３自由度、穿孔ロッド７ａの角度を決めるために２自由度、合計５自由度が必要となる。これに加え、操縦席からの視認性確保や、トンネル壁面との衝突回避、トンネル側壁面への穿孔を行うために、自由度に冗長性を持たせる必要があるため、合計８自由度を有している。そのため、例えば、穿孔ロッド１７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtから、その移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtをゼロとするための、８自由度に対応する関節、つまり第１～第８の関節１２ａ～１２ｈの動作速度の目標値を行列式で算出するようにした場合、入力と出力の数が異なるため、係数行列の逆行列を直接計算できない。したがって、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、穿孔ロッド７ａの姿勢を操作可能とすることが困難である。
Here, the
これに対し、実施形態に係る穿孔支援装置１によれば、第１～第３の関節１２ａ～１２ｃを駆動することで、他の関節の動作に起因して穿孔ロッド１７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtをゼロとし、穿孔ロッド７ａの先端部の位置を維持するようにした。それゆえ、上記（１）式では、予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtから、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出するようになる。そのため、式中で用いられる逆行列0Ｊ5(13)
-1は、３×３の正方行列となるため、直接計算できる。したがって、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、穿孔ロッド７ａの姿勢を操作可能とすることが容易となる。
On the other hand, according to the
（第２のモード実行処理）
次に、コントローラ１８（第１、第２の目標値演算部１８ａ、１８ｅ、第１、第２の関節群動作制御部１８ｄ、１８ｆ）が実行する第２のモード実行処理について説明する。第２のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第２のモードを表している場合に、所定時間（１０［msec］）が経過するたびに実行される。
図５に示すように、まず、ステップＳ２０１では、第２の目標値演算部１８ｅは、指示入力部１５が出力する第６の関節１２ｆの動作（回転）の指示入力に基づき、第６の関節１２ｆの角速度の目標値dθ6ref/dtを算出する。
(Second mode execution process)
Next, a second mode execution process executed by the controller 18 (first and second target
As shown in FIG. 5, first, in step S201, the second target
続いてステップＳ２０２に移行して、第１の目標値演算部１８ａは、ステップＳ２０１で算出した目標値dθ6ref/dtに基づき、第６の関節１２ｆの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtをゼロとするための、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する。具体的には、目標値dθ6ref/dtに基づき、上記（１）式に従って、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する。その際、第４、第５、第７の関節１２ｄ、１２ｅ、１２ｇの角速度の目標値ｄθ4ref/dt、ｄθ5ref/dt、ｄθ7ref/dtはゼロとする。
Subsequently, in step S202, the first target
続いてステップＳ２０３に移行して、第１の関節群動作制御部１８ｄは、ステップＳ２０２で算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dt、及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１、第２の関節１２ａ、１２ｂの角速度dθ1/dt、dθ2/dt、及び第３の関節１２ｃの伸縮速度dＬ3/dtを目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtと等しくするための指令を関節駆動装置８に出力する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第１～第３の関節１２ａ～１２ｃを駆動させ、第１～第３の関節１２ａ～１２ｃの動作速度dθ1/dt、dθ2/dt、dＬ3/dtを制御する。
Subsequently, in step S203, the first joint group
また同時に、第２の関節群動作制御部１８ｆは、ステップＳ２０１で算出した第６の関節１２ｆの角速度の目標値dθ6ref/dt、及び動作量検出部１３ｆの検出結果に基づき、第６の関節１２ｆの角速度dθ6/dtを目標値dθ6ref/dtと等しくするための指令を関節駆動装置８に出力した後、この演算処理、つまり、第２のモード実行処理を終了する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第６の関節１２ｆを駆動させ、第６の関節１２ｆの動作速度dθ6/dtを制御する。
これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１～第３、第６の関節１２ａ～１２ｃ、１２ｆそれぞれを駆動させることで、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、第６の回転角θ6を操作可能とする。その際、第７の関節１２ｇにおける第７の回転角θ7が０［rad］である場合には、穿孔ロッド７ａの先端部の位置に加え、穿孔ロッド７ａの姿勢も維持される。
At the same time, the second joint group
As a result, the
（第３のモード実行処理）
次に、コントローラ１８（第１、第２の目標値演算部１８ａ、１８ｅ、第１、第２の関節群動作制御部１８ｄ、１８ｆ）が実行する第３のモード実行処理について説明する。第３のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第３のモードを表している場合に、所定時間（１０［msec］）が経過するたびに実行される。
図６に示すように、まず、ステップＳ３０１では、第２の目標値演算部１８ｅは、指示入力部１５が出力する第７の関節１２ｇの動作（回転）の指示入力に基づき、第７の関節１２ｇの角速度の目標値dθ7ref/dtを算出する。
(Third mode execution process)
Next, a third mode execution process executed by the controller 18 (first and second target
As shown in FIG. 6, first, in step S301, the second target
続いてステップＳ３０２に移行して、第１の目標値演算部１８ａは、ステップＳ３０１で算出した目標値dθ7ref/dtに基づき、第７の回転角θ7の動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtをゼロとするための、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する。具体的には、目標値dθ7ref/dtに基づき、上記（１）式に従って、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する。その際、第４、第５、第６の関節１２ｄ、１２ｅ、１２ｆの角速度の目標値ｄθ4ref/dt、ｄθ5ref/dt、ｄθ6ref/dtはゼロとする。
Subsequently, in step S302, the first target
続いてステップＳ３０３に移行して、第１の関節群動作制御部１８ｄは、ステップＳ３０２で算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dt、及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１、第２の関節１２ａ、１２ｂの角速度dθ1/dt、dθ2/dt、及び第３の関節１２ｃの伸縮速度dＬ3/dtを目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtと等しくするための指令を関節駆動装置８に出力する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第１～第３の関節１２ａ～１２ｃを駆動させ、第１～第３の関節１２ａ～１２ｃの動作速度dθ1/dt、dθ2/dt、dＬ3/dtを制御する。
Subsequently, in step S303, the first joint group
また同時に、第２の関節群動作制御部１８ｆは、ステップＳ３０１で算出した第７の関節１２ｇの角速度の目標値dθ7ref/dt、及び動作量検出部１３ｇの検出結果に基づき、第７の関節１２ｇの角速度dθ7/dtを目標値dθ7ref/dtと等しくするための指令を関節駆動装置８に出力した後、この演算処理、つまり、第３のモード実行処理を終了する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第７の関節１２ｇを駆動させ、第７の関節１２ｇの動作速度dθ7/dtを制御する。
これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１～第３、第７の関節１２ａ～１２ｃ、１２gそれぞれを駆動させることで、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、第７の回転角θ7を操作可能とする。なお、第７の回転角θ7を操作する状況としては、ロックボルト穿孔がある。
At the same time, the second joint group
As a result, the
（第４のモード実行処理）
次に、コントローラ１８（第１の目標値演算部１８ａ、第１、第２の演算実行部１８ｂ、１８ｃ、第１の関節群動作制御部１８ｄ、第２の目標値演算部１８ｅ、第２の関節群動作制御部１８ｆ、位置取得部１８ｇ）が実行する第４のモード実行処理について説明する。第４のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第４のモードを表している場合に、所定時間（１０［msec］）が経過するたびに実行される。
図７に示すように、まず、ステップＳ４０１では、第２の目標値演算部１８ｅは、計画孔記憶部１６が記憶している切羽２の穿孔パターンから計画孔を１つ選択する。計画孔の選択方法としては、例えば、穿孔装置４のオペレータによる指示入力部１５の操作を基に選択する方法や、予め定めた順番で自動的に選択する方法を用いることができる。続いて、選択した計画孔の開口位置ｘref、ｙref、ｚref及び挿し角φref、ψrefを読み出す。
(Fourth mode execution process)
Next, the controller 18 (first target
As shown in FIG. 7, first, in step S401, the second target
続いてステップＳ４０２に移行して、第２の目標値演算部１８ｅは、Σ8座標系（後述）のＹ軸方向の単位ベクトル（R12、R22、R32）に基づき、下記（２）式に従って、切羽２の法線方向と穿孔ロッド７ａとがなす角度を挿し角φnow、ψnowとして算出する。挿し角φnowは、切羽２の法線方向と穿孔ロッド７ａとを上方から見た場合になす角度であり、挿し角ψnowは、切羽２の法線方向と穿孔ロッド７ａとを側方から見た場合になす角度である。続いて、図８に示すように、算出した挿し角φnow、ψnow、及びステップＳ４０１で読み出した計画孔の挿し角φref、ψrefに基づき、下記（３）式に従って、穿孔ロッド７ａの姿勢（挿し角φnow、ψnow）と計画孔の挿し角φref、ψrefとが等しくなるように、第４、第５の関節１２ｄの角速度の目標値dθ4ref/dt、dθ5ref/dtを算出する。
φnow=tan-1(Ｒ12/Ｒ22)
ψnow=tan-1(Ｒ32/Ｒ22) …（２）
dθ4ref/dt=ｋψ・ψerr
dθ5ref/dt=ｋφ・φerr …（３）
ψerr=ψref-ψnow
φerr=φref-φnow
ここで、ｋψ、ｋφは、フィードバックゲインである。
Subsequently, in step S402, the second target
φnow = tan -1 (R 12 / R 22 )
ψnow = tan -1 (R 32 / R 22 )… (2)
dθ 4 ref / dt = kψ ・ ψerr
dθ 5 ref / dt = kφ ・ φerr… (3)
ψerr = ψref-ψnow
φerr = φref-φnow
Here, kψ and kφ are feedback gains.
続いてステップＳ４０３に移行して、位置取得部１８ｇは、穿孔ロッド７ａの先端部の位置ｘnow、ｙnow、ｚnowを取得する。位置ｘnow、ｙnow、ｚnowを取得する方法としては、例えば、穿孔装置４とは別体に設けられ、穿孔ロッド７ａの先端部の位置を測量する測量装置（不図示）から取得する方法や、動作量検出部１３ａ～１３ｈの検出結果（回転量、伸縮量）に基づき、穿孔ロッド７ａの先端部の位置を算出する方法を採用できる。
続いてステップＳ４０４に移行して、第１の目標値演算部１８ａ（第１の演算実行部１８ｂ）は、ステップＳ４０１で読み出した計画孔の開口位置（ｘref、ｙref、ｚref）と、ステップＳ４０３で取得した穿孔ロッド７ａの先端部の位置（ｘnow、ｙnow、ｚnow）との差（距離）をゼロとするための、穿孔ロッド７ａの先端部の移動速度の目標値（以下、「目標速度Ｖref（=〔d0ｘ8ref/dt、d0ｙ8ref/dt、d0ｚ8ref/dt〕T）」とも呼ぶ）を算出する。
Subsequently, the process proceeds to step S403, and the
Subsequently, in step S404, the first target
具体的には、第１の目標値演算部１８ａ（第１の演算実行部１８ｂ）は、まず下記（４）式に従って正規化した位置誤差ベクトルＱerrを算出する。続いて、算出した正規化した位置誤差ベクトルＱerrに基づき、下記（５）式に従って目標速度Ｖrefを算出する。
Ｑerr=Ｐerr/Ｄerr …（４）
Ｐerr=〔ｘerr、ｙerr、ｚerr〕T
Ｄerr=〔ｘerr2+ｙerr2+ｚerr2〕1/2
ｘerr=ｘref-ｘnow
ｙerr=ｙref-ｙnow
ｚerr=ｚref-ｚnow
Ｖref=Ｑerr・Ｓref …（５）
Specifically, the first target
Qerr = Perr / Derr ... (4)
Perr = [xerr, yerr, zerr] T
Derr = [xerr 2 + yerr 2 + zerr 2 ] 1/2
xerr = xref-xnow
yerr = yref-ynow
zerr = zref-znow
Vref = Qerr ・ Sref… (5)
ここで、Ｓrefは、穿孔ロッド７ａの先端部の移動速度の目標値の大きさを設定するためのスカラ値である。スカラ値Ｓrefとしては、例えば、加速域、定速域及び減速域からなる速度パターンを設定するようにしてもよい。加速域では、第１の関節群動作制御部１８ｄによる制御の開始直後に、穿孔ロッド７ａの先端部の移動速度の目標値d0ｘ8ref/dt、d0ｙ8ref/dt、d0ｚ8ref/dtの時間変化率を徐々に増大させ、定速域では、目標値d0ｘ8ref/dt、d0ｙ8ref/dt、d0ｚ8ref/dtが予め定められた最大速度に到達すると最大速度を維持させ、減速域では、第１の関節群動作制御部１８ｄによる制御の終了直前に、目標値d0ｘ8ref/dt、d0ｙ8ref/dt、d0ｚ8ref/dtの時間変化率を徐々に低減させる。これにより、穿孔ロッド７ａの先端部の移動開始時に移動速度をスローアップし、移動終了時に移動速度をスローダウンすることができるため、第１～第３の関節１２ａ～１２ｃの振動を抑制することができる。
Here, Sref is a scalar value for setting the magnitude of the target value of the moving speed of the tip portion of the
ちなみに、穿孔装置４では、第１～第３の関節１２ａ～１２ｃを、電動モータ等と異なり応答速度が遅い油圧駆動装置を用いて駆動している。それゆえ、例えば、目標値d0ｘ8ref/dt、d0ｙ8ref/dt、d0ｚ8ref/dtの時間変化率が増大し、第１～第３の関節１２ａ～１２ｃの動作速度の目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtが増大すると、応答遅れやオーバーシュートが発生して、第１～第３の関節１２ａ～１２ｃに振動が発生する可能性がある。
Incidentally, in the
続いてステップＳ４０５に移行して、第１の目標値演算部１８ａ（第２の演算実行部１８ｃ）は、ステップＳ４０２で算出した目標値dθ4ref/dt、dθ5ref/dt、及びステップＳ４０４で算出した目標速度Ｖrefに基づき、第４、第５の関節１２ｄ、１２ｅの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtをゼロとするとともに、計画孔の開口位置（ｘref、ｙref、ｚref）と穿孔ロッド７ａの先端部の位置（ｘnow、ｙnow、ｚnow）とを等しくするための、第１～第３の関節１２ａ～１２ｃの動作速度の目標値〔dθ1ref/dt、dθ2ref/dt、dＬ3ref/dt〕Tを算出する。
Subsequently, in step S405, the first target
具体的には、第１の目標値演算部１８ａ（第２の演算実行部１８ｃ）は、まず、目標値dθ4ref/dt、dθ5ref/dtに基づき、下記（６）式に従って穿孔ロッド７ａの先端部の移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtを算出する。続いて、算出した移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dt、及び目標速度Ｖrefに基づき、下記（７）式に従って、第１～第３の関節１２ａ～１２ｃの動作速度の目標値〔dθ1ref/dt、dθ2ref/dt、dＬ3ref/dt〕Tを算出する。
〔d0ｘ8ref/dt、d0ｙ8ref/dt、d0ｚ8ref/dt〕T
=0Ｊ8(48)・〔dθ4ref/dt、dθ5ref/dt、０、０〕T …（６）
〔dθ1ref/dt、dθ2ref/dt、dＬ3ref/dt〕T
=0Ｊ5(13)
-1・(Ｖref-〔d0ｘ8ref/dt、d0ｙ8ref/dt、d0ｚ8ref/dt〕T） …（７）
Specifically, the first target
[D 0 x 8 ref / dt, d 0 y 8 ref / dt, d 0 z 8 ref / dt] T
= 0 J 8 (48) · [dθ 4 ref / dt, dθ 5 ref / dt, 0, 0] T … (6)
[Dθ 1 ref / dt, dθ 2 ref / dt, dL 3 ref / dt] T
= 0 J 5 (13) -1 · (Vref-[d 0 x 8 ref / dt, d 0 y 8 ref / dt, d 0 z 8 ref / dt] T )… (7)
続いてステップＳ４０６に移行して、第１の関節群動作制御部１８ｄは、ステップＳ４０５で算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dt、及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１、第２の関節１２ａ、１２ｂの角速度dθ1/dt、dθ2/dt、及び第３の関節１２ｃの伸縮速度dＬ3/dtを目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtと等しくするための指令を関節駆動装置８に出力する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第１～第３の関節１２ａ～１２ｃを駆動させ、第１～第３の関節１２ａ～１２ｃの動作速度dθ1/dt、dθ2/dt、dＬ3/dtを制御する。
Subsequently, in step S406, the first joint group
同時に、第２の関節群動作制御部１８ｆは、ステップＳ４０２で算出した第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/dt、dθ5ref/dt、及び動作量検出部１３ｄ、１３ｅの検出結果に基づき、第４、第５の関節１２ｄ、１２ｅの角速度dθ4/dt、dθ5/dtを目標値dθ4ref/dt、dθ5ref/dと等しくするための指令を関節駆動装置８に出力した後、この演算処理、つまり、第４のモード実行処理を終了する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第４、第５の関節１２ｄ、１２ｅを駆動させ、第４、第５の関節１２ｄ、１２ｅの動作速度dθ4/dt、dθ5/dtを制御する。
これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１～第５の関節１２ａ～１２ｅそれぞれを駆動させることで、穿孔ロッド７ａの先端部の位置と姿勢とを、計画孔の開口位置と挿し角とに自動的にゆっくりと徐々に近づけることができる。
At the same time, the second joint group
As a result, the
（第５のモード実行処理）
次に、コントローラ１８（第１、第２の目標値演算部１８ａ、１８ｅ、第１、第２の関節群動作制御部１８ｄ、１８ｆ）が実行する第５のモード実行処理について説明する。第５のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第５のモードを表している場合に、所定時間（１０［msec］）が経過するたびに実行される。
図９に示すように、まず、ステップＳ５０１では、第１の目標値演算部１８ａは、指示入力部１５が出力する第１～第３の関節１２ａ～１２ｃの動作の指示入力に基づき、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する。また、第２の目標値演算部１８ｅは、指示入力部１５が出力する第４～第７の関節１２ｄ～１２ｇの動作の指示入力に基づき、第４～第７の関節１２ｄ～１２ｇの角速度の目標値dθ4ref/dt～dθ7ref/dtを算出する。
(Fifth mode execution process)
Next, a fifth mode execution process executed by the controller 18 (first and second target
As shown in FIG. 9, first, in step S501, the first target
続いてステップＳ５０２に移行して、第１の関節群動作制御部１８ｄは、ステップＳ５０１で算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dt、及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１、第２の関節１２ａ、１２ｂの角速度dθ1/dt、dθ2/dt、及び第３の関節１２ｃの伸縮速度dＬ3/dtを目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtと等しくするための指令を関節駆動装置８に出力する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第１～第３の関節１２ａ～１２ｃを駆動させ、第１～第３の関節１２ａ～１２ｃの動作速度dθ1/dt、dθ2/dt、dＬ3/dtを制御する。
Subsequently, in step S502, the first joint group
また同時に、第２の関節群動作制御部１８ｆは、ステップＳ５０１で算出した第４～第７の関節１２ｄ～１２ｇの角速度の目標値dθ4ref/d～dθ7ref/dt、及び動作量検出部１３ｄ～１３ｇの検出結果に基づき、第４～第７の関節１２ｄ～１２ｇの角速度dθ4/dt～dθ7/dtを目標値dθ4ref/dt～dθ7ref/dtと等しくするための指令を関節駆動装置８に出力した後、この演算処理、つまり、第５のモード実行処理を終了する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第４～第７の関節１２ｄ～１２ｇを駆動させ、第４～第７の関節１２ｄ～１２ｇの動作速度dθ4/dt～dθ7/dtを制御する。
At the same time, the second joint group
（演算式の導出手順）
次に、上記（１）式の導出手順について詳細に説明する。
まず、図１０（ａ）（ｂ）に示すように、Σ0～Σ8座標系の原点を設定する。そして、図１０（ａ）（ｂ）に示すように、第１、第２、第４～第７の回転角θ1、θ2、θ4～θ7が０［rad］であるときに、Σ0～Σ8座標系のＸ方向が移動台車５の右方向と一致し、Σ0～Σ8座標系のＹ方向が移動台車５の前方向と一致し、Σ0～Σ8座標系のＺ方向が移動台車５の上方向と一致するようにした。また、Σ0座標系からΣ1座標系の原点を見た場合の位置、Σ1座標系からΣ2座標系の原点を見た場合の位置の位置、Σ2座標系からΣ3座標系の原点を見た場合の位置、Σ4座標系からΣ5座標系の原点を見た場合の位置、Σ5座標系からΣ6座標系の原点を見た場合の位置、Σ6座標系からΣ7座標系の原点を見た場合の位置、並びにΣ7座標系からΣ8座標系の原点を見た場合の位置を、下記表１に示す。
(Procedure for deriving arithmetic expressions)
Next, the procedure for deriving the above equation (1) will be described in detail.
First, as shown in FIGS. 10 (a) and 10 (b), the origin of the Σ 0 to Σ 8 coordinate system is set. Then, as shown in FIGS. 10A and 10B, when the first, second, fourth to seventh rotation angles θ 1 , θ 2 , and θ 4 to θ 7 are 0 [rad], The X direction of the Σ 0 to Σ 8 coordinate system coincides with the right direction of the moving trolley 5, and the Y direction of the Σ 0 to Σ 8 coordinate system coincides with the front direction of the moving trolley 5 . The Z direction coincides with the upward direction of the moving carriage 5. Also, the position when looking at the origin of the Σ 1 coordinate system from the Σ 0 coordinate system, the position of the position when looking at the origin of the Σ 2 coordinate system from the Σ 1 coordinate system, and the position when looking at the origin of the Σ 2 coordinate system from the Σ 2 coordinate system. Position when looking at the origin, position when looking at the origin of the Σ 5 coordinate system from the Σ 4 coordinate system, position when looking at the origin of the Σ 6 coordinate system from the Σ 5 coordinate system, Σ from the Σ 6 coordinate system Table 1 below shows the positions when the origin of the 7 -coordinate system is viewed, and the positions when the origin of the Σ 8 coordinate system is viewed from the Σ 7 coordinate system.
すると、Σ0座標系とΣ1座標系との同次変換行列0Ｔ1は、下記（８）式で表される。
ここで、C1=cosθ1、S1=sinθ1である。
また、Σ1座標系とΣ2座標系との同次変換行列1Ｔ2は、下記（９）式で表される。
Further, the homogeneous transformation matrix 1 T 2 between the Σ 1 coordinate system and the Σ 2 coordinate system is expressed by the following equation (9).
ここで、C2=cosθ2、S2=sinθ2である。
また、Σ2座標系とΣ3座標系との同次変換行列2Ｔ3は、下記（１０）式で表される。
Further, the homogeneous transformation matrix 2 T 3 between the Σ 2 coordinate system and the Σ 3 coordinate system is expressed by the following equation (10).
また、Σ3座標系とΣ4座標系との同次変換行列3Ｔ4は、下記（１１）式で表される。
ここで、C4=cosθ4、S4=sinθ4である。
また、Σ4座標系とΣ5座標系との同次変換行列4Ｔ5は、下記（１２）式で表される。
Further, the homogeneous transformation matrix 4 T 5 between the Σ 4 coordinate system and the Σ 5 coordinate system is expressed by the following equation (12).
ここで、C5=cosθ5、S5=sinθ5である。
また、Σ5座標系とΣ6座標系との同次変換行列5Ｔ6は、下記（１３）式で表される。
Further, the homogeneous transformation matrix 5 T 6 between the Σ 5 coordinate system and the Σ 6 coordinate system is expressed by the following equation (13).
ここで、C6=cosθ6、S6=sinθ6である。
また、Σ6座標系とΣ7座標系との同次変換行列6Ｔ7は、下記（１４）式で表される。
Further, the homogeneous transformation matrix 6 T 7 between the Σ 6 coordinate system and the Σ 7 coordinate system is expressed by the following equation (14).
ここで、C7=cosθ7、S7=sinθ7である。
また、Σ7座標系とΣ8座標系との同次変換行列7Ｔ8は、下記（１５）式で表される。
Further, the homogeneous transformation matrix 7 T 8 between the Σ 7 coordinate system and the Σ 8 coordinate system is expressed by the following equation (15).
また、Σ0座標系とΣ8座標系との同次変換行列0Ｔ8は、下記（１６）式で表される。
以上より、Σ0座標系から見たΣ8座標系の原点の座標は下記（１７）式で表される。
-S1(
(-S2(
S4(S5(S6(Ｌ8S7+C7Hshell+Hdump)-Drote-C6Ddump)+C5((-HshellS7)+Lrote+C7Ｌ8)+ Ltilt)
+ C4(C6(Ｌ8S7+C7Hshell+Hdump)+DdumpS6+Htilt)-Hfork
))
+C2(
C4(S5(S6(Ｌ8S7+C7Hshell+Hdump)-Drote-C6Ddump)+C5((-HshellS7)+Lrote+C7Ｌ8)+Ltilt)
-S4(C6(Ｌ8S7+C7Hshell+Hdump)+DdumpS6+Htilt)+Ｌ3
)+Lpede
)
0ｙ8=C1(
(-S2(
S4(S5(S6(Ｌ8S7+C7Hshell+Hdump)-Drote-C6Ddump)+C5((-HshellS7)+Lrote+C7Ｌ8)+Ltilt)
+C4(C6(Ｌ8S7+C7Hshell+Hdump)+DdumpS6+Htilt)-Hfork
))
+ C2(
C4(S5(S6(Ｌ8S7+C7Hshell+Hdump)-Drote-C6Ddump)+C5((-HshellS7)+Lrote+C7Ｌ8)+Ltilt)
-S4(C6(Ｌ8S7+C7Hshell+Hdump)+DdumpS6+Htilt)+Ｌ3
)+Lpede
)
+S1(C5(S6(Ｌ8S7+C7Hshell+Hdump)-Drote-C6Ddump)-S5((-HshellS7)+Lrote+C7Ｌ8))
+ Lyork
0ｚ8=C2(
S4(S5(S6(Ｌ8S7+C7Hshell+Hdump)-Drote-C6Ddump)+C5((-HshellS7)+Lrote+C7Ｌ8)+Ltilt)
+C4(C6(Ｌ8S7+C7Hshell+Hdump)+DdumpS6+Htilt)-Hfork
)
+S2(
C4(S5(S6(Ｌ8S7+C7Hshell+Hdump)-Drote-C6Ddump)+C5((-HshellS7)+Lrote+C7Ｌ8)+Ltilt)
-S4(C6(Ｌ8S7+C7Hshell+Hdump)+DdumpS6+Htilt)+Ｌ3
)+Hyork
From the above, the coordinates of the origin of the Σ 8 coordinate system seen from the Σ 0 coordinate system are expressed by the following equation (17).
-S 1 (
(-S 2 (
S 4 (S 5 (S 6 (L 8 S 7 + C 7 H shell + H dump )-D rote -C 6 D dump ) + C 5 ((-H shell S 7 ) + L rote + C 7 L 8 ) + L tilt )
+ C 4 (C 6 (L 8 S 7 + C 7 H shell + H dump ) + D dump S 6 + H tilt )-H fork
))
+ C 2 (
C 4 (S 5 (S 6 (L 8 S 7 + C 7 H shell + H dump )-D rote -C 6 D dump ) + C 5 ((-H shell S 7 ) + L rote + C 7 L 8 ) + L tilt )
-S 4 (C 6 (L 8 S 7 + C 7 H shell + H dump ) + D dump S 6 + H tilt ) + L 3
) + L pede
)
0 y 8 = C 1 (
(-S 2 (
S 4 (S 5 (S 6 (L 8 S 7 + C 7 H shell + H dump )-D rote -C 6 D dump ) + C 5 ((-H shell S 7 ) + L rote + C 7 L 8 ) + L tilt )
+ C 4 (C 6 (L 8 S 7 + C 7 H shell + H dump ) + D dump S 6 + H tilt )-H fork
))
+ C 2 (
C 4 (S 5 (S 6 (L 8 S 7 + C 7 H shell + H dump )-D rote -C 6 D dump ) + C 5 ((-H shell S 7 ) + L rote + C 7 L 8 ) + L tilt )
-S 4 (C 6 (L 8 S 7 + C 7 H shell + H dump ) + D dump S 6 + H tilt ) + L 3
) + L pede
)
+ S 1 (C 5 (S 6 (L 8 S 7 + C 7 H shell + H dump )-D rote -C 6 D dump )-S 5 ((-H shell S 7 ) + L rote + C 7 L 8 ))
+ L york
0 z 8 = C 2 (
S 4 (S 5 (S 6 (L 8 S 7 + C 7 H shell + H dump )-D rote -C 6 D dump ) + C 5 ((-H shell S 7 ) + L rote + C 7 L 8 ) + L tilt )
+ C 4 (C 6 (L 8 S 7 + C 7 H shell + H dump ) + D dump S 6 + H tilt )-H fork
)
+ S 2 (
C 4 (S 5 (S 6 (L 8 S 7 + C 7 H shell + H dump )-D rote -C 6 D dump ) + C 5 ((-H shell S 7 ) + L rote + C 7 L 8 ) + L tilt )
-S 4 (C 6 (L 8 S 7 + C 7 H shell + H dump ) + D dump S 6 + H tilt ) + L 3
) + H york
ここで、各関節１２ａ～１２ｈの状態量がθ1、θ2、Ｌ3、θ4、θ5、θ6、θ7、Ｌ8であるときに、第４～第７の関節１２ｄ～１２ｇを角速度dθ4/dt、dθ5/dt、dθ6/dt、dθ7/dtで回転させた場合、穿孔ロッド７ａの先端に発生する移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtは、穿孔ロッド７ａの先端部の座標（0x8、0y8、0z8）、つまり上記（１７）式を第４～第７の回転角θ4～θ7で偏微分し、偏微分したものに、第４～第７の関節１２ｄ～１２ｇの角速度dθ4/dt、dθ5/dt、dθ6/dt、dθ7/dtを乗算することで得られる。
Here, when the state quantities of the
すなわち、チルトボディ９ｅを第４の関節１２ｄ回りに角速度dθ4/dt で回転させたときに、穿孔ロッド７ａの先端に発生する移動速度のＸ方向成分d0ｘ8/dt、Ｙ方向成分d0ｙ8/dt、Ｚ方向成分d0ｚ8/dtは、下記（１８）～（２０）式で表される。
d0ｘ8/dt=〔-S1(C2(-S4P4A-C4P4B)-S2(C4P4A-S4P4B))〕・dθ4/dt …（１８）
d0ｙ8/dt=〔C1(C2(-S4P4A-C4P4B)-S2(C4P4A-S4P4B))〕・dθ4/dt …（１９）
d0ｚ8/dt=〔S2(-S4P4A-C4P4B)+C2(C4P4A-S4P4B)〕・dθ4/dt …（２０）
P4A=S5(S6(S7Ｌ8+C7Hshell+Hdump)-C6Ddump-Drote+C5(-S7Hshell+Lrote+C7Ｌ8)+Ltilt、P4B=C6(S7Ｌ8+C7Hshell+Hdump)+S6Ddump+Htilt
That is, when the
d 0 x 8 / dt = [-S 1 (C 2 (-S 4 P 4A -C 4 P 4B )-S 2 (C 4 P 4A -S 4 P 4B ))] · d θ 4 / dt… (18 )
d 0 y 8 / dt = [C 1 (C 2 (-S 4 P 4A -C 4 P 4B ) -S 2 (C 4 P 4A -S 4 P 4B ))] · d θ 4 / dt… (19)
d 0 z 8 / dt = [S 2 (-S 4 P 4A -C 4 P 4B ) + C 2 (C 4 P 4A -S 4 P 4B )] ・ d θ 4 / dt… (20)
P 4A = S 5 (S 6 (S 7 L 8 + C 7 H shell + H dump )-C 6 D dump -D rote + C 5 (-S 7 H shell + L rote + C 7 L 8 ) + L tilt , P 4B = C 6 (S 7 L 8 + C 7 H shell + H dump ) + S 6 D dump + H tilt
また、スイングボディ９ｆを第５の関節１２ｅ回りに角速度dθ5/dtで回転させたときに、穿孔ロッド７ａの先端に発生する移動速度のＸ方向成分d0ｘ8/dt、Ｙ方向成分d0ｙ8/dt、Ｚ方向成分d0ｚ8/dtは、下記（２１）～（２３）式で表される。
d0ｘ8/dt=〔C1 P5B -S1(C2 C4P5A -S2 S4 P5A)〕・dθ5/dt …（２１）
d0ｙ8/dt=〔S1 P5B +C1(C2 C4P5A -S2 S4 P5A)〕・dθ5/dt …（２２）
d0ｚ8/dt=〔C2 S4 P5A + S2 C4P5A〕・dθ5/dt …（２３）
P5A= C5 (S6 ( S7Ｌ8+ C7 Hshell + Hdump) - C6 Ddump- Drote)- S5 (-S7 Hshell + Lrote+ C7Ｌ8)、P5B= - S5 (S6 ( S7Ｌ8 + C7 Hshell+ Hdump) - C6 Ddump - Drote)- C5(-S7 Hshell + Lrote + C7Ｌ8)
Further, when the
d 0 x 8 / dt = [C 1 P 5B -S 1 (C 2 C 4 P 5A -S 2 S 4 P 5A )] ・ dθ 5 / dt… (21)
d 0 y 8 / dt = [S 1 P 5B + C 1 (C 2 C 4 P 5A -S 2 S 4 P 5A )] ・ d θ 5 / dt… (22)
d 0 z 8 / dt = [C 2 S 4 P 5A + S 2 C 4 P 5A ] ・ d θ 5 / dt… (23)
P 5A = C 5 (S 6 (S 7 L 8 + C 7 H shell + H dump ) --C 6 D dump --D rote )-S 5 (-S 7 H shell + L rote + C 7 L 8 ), P 5B = --S 5 (S 6 (S 7 L 8 + C 7 H shell + H dump ) --C 6 D dump --D rote )-C 5 (-S 7 H shell + L rote + C 7 L 8 )
さらに、ロータリーボディ９ｇを第６の関節１２ｆ回りに角速度dθ6/dtで回転させたときに、穿孔ロッド７ａの先端に発生する移動速度のＸ方向成分d0ｘ8/dt、Ｙ方向成分d0ｙ8/dt、Ｚ方向成分d0ｚ8/dtは、下記（２４）～（２６）式で表される。
d0ｘ8/dt=〔C1C5P6A-S1(C2(C4S5P6A-S4P6B)-S2(C4P6B+S4S5P6A))〕・dθ6/dt …（２４）
d0ｙ8/dt=〔C1(C2(C4S5P6A-S4P6B)-S2(C4P6B+S4S5P6A))+C5S1P6A〕・dθ6/dt …（２５）
d0ｚ8/dt=〔S2(C4S5P6A-S4P6B)+C2(C4P6B+S4S5P6A)〕・dθ6/dt …（２６）
P6A=C6(S7Ｌ8+C7Hshell+Hdump)+S6Ddump、P6B=-S6(S7Ｌ8+C7Hshell+Hdump)+C6Ddump
Further, when the
d 0 x 8 / dt = [C 1 C 5 P 6A -S 1 (C 2 (C 4 S 5 P 6A -S 4 P 6B ) -S 2 (C 4 P 6B + S 4 S 5 P 6A )) ] ・ Dθ 6 / dt… (24)
d 0 y 8 / dt = [C 1 (C 2 (C 4 S 5 P 6A -S 4 P 6B )-S 2 (C 4 P 6B + S 4 S 5 P 6A )) + C 5 S 1 P 6A ] ・ Dθ 6 / dt… (25)
d 0 z 8 / dt = [S 2 (C 4 S 5 P 6A -S 4 P 6B ) + C 2 (C 4 P 6B + S 4 S 5 P 6A )] ・ dθ 6 / dt… (26)
P 6A = C 6 (S 7 L 8 + C 7 H shell + H dump ) + S 6 D dump , P 6B =-S 6 (S 7 L 8 + C 7 H shell + H dump ) + C 6 D dump
また、ガイドマウンチング９ｉを第７の関節１２ｇ回りに角速度dθ7/dtで回転させたときに、穿孔ロッド７ａの先端に発生する移動速度のＸ方向成分d0ｘ8/dt、Ｙ方向成分d0ｙ8/dt、Ｚ方向成分d0ｚ8/dtは、下記（２７）～（２９）式で表される。
d0ｘ8/dt=〔C1(C5S6P7A-S5P7B)-S1(C2(C4(C5P7B+S5S6P7A)-C6S4P7A)-S2(S4(C5P7B+S5S6P7A)+C4C6P7A))〕・dθ7/dt …（２７）
d0ｙ8/dt=〔C1(C2(C4(C5P7B+S5S6P7A)-C6S4P7A)-S2(S4(C5P7B+S5S6P7A)+C4C6P7A))+S1(C5S6P7A-S5P7B)〕・dθ7/dt …（２８）
d0ｚ8/dt=〔C2(S4(C5P7B+S5S6P7A)+C4C6P7A)+S2(C4(C5P7B+S5S6P7A)-C6S4P7A)〕・dθ7/dt …（２９）
P7A=C7Ｌ8-S7Hshell、P7B=-S7Ｌ8-C7Hshell
Further, when the guide mounting 9i is rotated around the 7th joint 12 g at an angular velocity dθ 7 / dt, the X-direction component d 0 x 8 / dt and the Y-direction component of the movement speed generated at the tip of the
d 0 x 8 / dt = [C 1 (C 5 S 6 P 7A -S 5 P 7B )-S 1 (C 2 (C 4 (C 5 P 7B + S 5 S 6 P 7A ))-C 6 S 4 P 7A )-S 2 (S 4 (C 5 P 7B + S 5 S 6 P 7A ) + C 4 C 6 P 7A ))] ・ dθ 7 / dt… (27)
d 0 y 8 / dt = [C 1 (C 2 (C 4 (C 5 P 7B + S 5 S 6 P 7A )-C 6 S 4 P 7A )-S 2 (S 4 (C 5 P 7B + S) 5 S 6 P 7A ) + C 4 C 6 P 7A )) + S 1 (C 5 S 6 P 7A -S 5 P 7B )] ・ dθ 7 / dt… (28)
d 0 z 8 / dt = [C 2 (S 4 (C 5 P 7B + S 5 S 6 P 7A ) + C 4 C 6 P 7A ) + S 2 (C 4 (C 5 P 7B + S 5 S 6 ) P 7A )-C 6 S 4 P 7A )] ・ dθ 7 / dt… (29)
P 7A = C 7 L 8 -S 7 H shell , P 7B = -S 7 L 8 -C 7 H shell
また、第４～第７の関節１２ｄ～１２ｇの動作を同時に行った場合には、上記（１８）～（２９）式のＸ方向成分d0ｘ8/dt、Ｙ方向成分d0ｙ8/dt、Ｚ方向成分d0ｚ8/dtを成分毎に足し合わせた速度が、穿孔ロッド７ａの先端部に発生する。それゆえ、穿孔ロッド７ａの先端部に発生する移動速度のＸ方向成分d0ｘ8/dt、Ｙ方向成分d0ｙ8/dt、Ｚ方向成分d0ｚ8/dtは、下記（３０）式に示すように、行列式で表すことができる。
Further, when the operations of the 4th to
ここで、第４～第７の関節１２ｄ～１２ｇの角速度dθ4/dt、dθ5/dt、dθ6/dt、dθ7/dtを穿孔ロッド７ａの先端の移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtへ変換する行列0J8(48)は、ヤコビ行列と呼ばれる。ヤコビ行列0J8(48)は、各関節１２ａ～１２ｈの状態量（回転量、伸縮量）θ1、θ2、Ｌ3、θ4、θ5、θ6、θ7、Ｌ8によって変化するため、穿孔ロッド７ａの先端部の位置や姿勢が変化した場合には更新する必要がある。なお、穿孔ロッド７ａの先端部の位置や姿勢の変化が小さい場合には、ヤコビ行列0J8(48)の変化も小さくなる。そのため、必ずしも制御周期毎にヤコビ行列0J8(48)を更新する必要はない。
Here, the angular velocities dθ 4 / dt, dθ 5 / dt, dθ 6 / dt, and dθ 7 / dt of the 4th to
また、第１～第８の関節１２ａ～１２ｈの状態量がθ1、θ2、Ｌ3、θ4、θ5、θ6、θ7、Ｌ8であるときに、第１～第３の関節１２ａ～１２ｃを角速度dθ1/dt、dθ2/dt、伸縮速度dＬ3/dtで動作させた場合、位置合わせの基点に発生する移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtを算出するための数式も、上記（３０）式の導出手順と同様の手順によって得られる。ただし、穿孔装置４は、角速度dθ2/dtに対して角速度dθ4/dtが同調するため、角速度dθ2/dtによって発生する基点の移動速度は、（d0ｘ5/dt、d0ｙ5/dt、d0ｚ5/dt）=（d0ｘ3/dt、d0ｙ3/dt、d0ｚ3/dt）となる。また、第３の関節１２ｃの伸縮速度dＬ3/dtによって発生する基点の移動速度は、ブーム９の姿勢によらず、（d0ｘ5/t、d0ｙ5/t、d0ｚ5/t）=（d0ｘ3/dt、d0ｙ3/dt、d0ｚ3/dt）となる。
ここで、基点の座標の各成分0x5、0y5は、下記（３１）、（３２）式で表される。なお、0z5は、θ1の要素を持たず、θ1で偏微分すると０となるため説明を省略する。
0x5=0x3+S1(S4(S2Ltilt+C2Htilt)+C4(-C2Ltilt+S2Htilt)) …（３１）
0y5=0y3+C1(-S4(S2Ltilt+C2Htilt)-C4(-C2Ltilt+S2Htilt)) …（３２）
0x3=-S1C2Ｌ3-S1S2Hfork-S1Lpede
0y3=C1C2Ｌ3+C1S2Hfork+C1Lpede+Lyork
0z3=S2Ｌ3-C2Hfork+Hyork
Further, when the state quantities of the first to
Here, each component 0 x 5 and 0 y 5 of the coordinates of the base point are expressed by the following equations (31) and (32). Note that 0 z 5 does not have an element of θ 1 and becomes 0 when partially differentiated with respect to θ 1 , so the description thereof will be omitted.
0 x 5 = 0 x 3 + S 1 (S 4 (S 2 L tilt + C 2 H tilt ) + C 4 (-C 2 L tilt + S 2 H tilt ))… (31)
0 y 5 = 0 y 3 + C 1 (-S 4 (S 2 L tilt + C 2 H tilt )-C 4 (-C 2 L tilt + S 2 H tilt ))… (32)
0 x 3 =-S 1 C 2 L 3 -S 1 S 2 H fork -S 1 L pede
0 y 3 = C 1 C 2 L 3 + C 1 S 2 H fork + C 1 L pede + L york
0 z 3 = S 2 L 3 -C 2 H fork + H york
続いて、上記（３１）、（３２）式及び省略した0ｚ5の数式を第１の回転角θ1、θ2や第３の伸縮量Ｌ3で偏微分し、偏微分したものに、第１～第３の関節１２ａ～１２ｃの角速度dθ1/dt、dθ5/dtやdＬ3/dを乗算すると、下記（３３）～（３５）式が得られる。
d0ｘ5/dt=〔(-C1S2Hfork-C1Lpede-C1C2Ｌ3)+C1(S4(S2Ltilt+C2Htilt)+C4(-C2Ltilt+S2Htilt))〕・dθ1/dt
d0ｙ5/dt=〔(-S1S2Hfork-S1Lpede-S1C2Ｌ3)+S1(S4(S2Ltilt+C2Htilt)+C4(-C2Ltilt+S2Htilt))〕・dθ1/dt
d0ｚ5/dt=0 …（３３）
d0ｘ5/dt=〔S1S2Ｌ3-S1C2Hfork〕・dθ2/dt
d0ｙ5/dt=〔-C1S2Ｌ3＋C1C2Hfork〕・dθ2/dt
d0ｚ5/dt=〔C2Ｌ3＋S2Hfork〕・dθ2/dt …（３４）
d0ｘ5/dt=〔-S1C2〕・dＬ3/dt
d0ｙ5/dt=〔C1C2〕・dＬ3/dt
d0ｚ5/dt=〔S2〕・dＬ3/dt …（３５）
Subsequently, the above equations (31) and (32) and the omitted equation of 0 z 5 are partially differentiated with respect to the first rotation angle θ 1 , θ 2 and the third expansion / contraction amount L 3 , and are partially differentiated. Multiplying the angular velocities dθ 1 / dt, dθ 5 / dt and dL 3 / d of the first to
d 0 x 5 / dt = [(-C 1 S 2 H fork -C 1 L pede -C 1 C 2 L 3 ) + C 1 (S 4 (S 2 L tilt + C 2 H tilt ) + C 4 ( -C 2 L tilt + S 2 H tilt ))] ・ dθ 1 / dt
d 0 y 5 / dt = [(-S 1 S 2 H fork -S 1 L pede -S 1 C 2 L 3 ) + S 1 (S 4 (S 2 L tilt + C 2 H tilt ) + C 4 ( -C 2 L tilt + S 2 H tilt ))] ・ dθ 1 / dt
d 0 z 5 / dt = 0… (33)
d 0 x 5 / dt = [S 1 S 2 L 3 -S 1 C 2 H fork ] ・ d θ 2 / dt
d 0 y 5 / dt = [-C 1 S 2 L 3 + C 1 C 2 H fork ] ・ d θ 2 / dt
d 0 z 5 / dt = [C 2 L 3 + S 2 H fork ] ・ d θ 2 / dt… (34)
d 0 x 5 / dt = [-S 1 C 2 ] ・ dL 3 / dt
d 0 y 5 / dt = [C 1 C 2 ] ・ dL 3 / dt
d 0 z 5 / dt = [S 2 ] ・ dL 3 / dt… (35)
また、第１～第３の関節１２ａ～１２ｃの動作を同時に行った場合には、上記（３３）～（３５）式のＸ方向成分d0ｘ5/dt、Ｙ方向成分d0ｙ5/dt、Ｚ方向成分d0ｚ5/dtを成分毎に足し合わせた速度が、位置合わせの基点に発生する。それゆえ、基点に発生する移動速度のＸ方向成分d0ｘ5/dt、Ｙ方向成分d0ｙ5/dt、Ｚ方向成分d0ｚ5/dtは、上記（３０）式と同様に、下記（３６）式に示すように、行列式で表すことができる。
Further, when the operations of the first to
ここで、第１～第３の関節１２ａ～１２ｃの角速度dθ1/dt、dθ2/dt、dＬ3/dtを位置合わせの基点の移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtへ変換する行列0J5(13)もヤコビ行列と呼ばれる。また、ヤコビ行列0J5(15)は、正方行列であるため、逆行列0J5(15)
-1を求めることができる。ヤコビ逆行列0J5(15)
-1を用いることで、位置合わせの基点における速度（d0ｘ5/dt、d0ｙ5/dt、d0ｚ5/dt）を実現するための、第１～第３の関節１２ａ～１２ｃの動作速度（dθ1/dt、dθ2/dt、dＬ3/dt）を演算可能な下記（３７）式が導出される。
Here, the moving speeds of the base points for aligning the angular velocities dθ 1 / dt, dθ 2 / dt, and dL 3 / dt of the first to
k21=S1/(S4(S2Ltilt+C2Htilt)+C4(S2Htilt-C2Ltilt)-S2Hfork-C2Ｌ3-Lpede)
k31=0
k12=(S1S2)/Ｌ3
k22=-(C1S2)/Ｌ3
k32=C2/Ｌ3
k13=-(S1S2Hfork/Ｌ3+S1C2)
k23=(C1S2Hfork/Ｌ3+C1C2)
k33=(-C2Hfork/Ｌ3+S2)
k 21 = S 1 / (S 4 (S 2 L tilt + C 2 H tilt ) + C 4 (S 2 H tilt -C 2 L tilt )-S 2 H fork -C 2 L 3 -L pede )
k 31 = 0
k 12 = (S 1 S 2 ) / L 3
k 22 =-(C 1 S 2 ) / L 3
k 32 = C 2 / L 3
k 13 =-(S 1 S 2 H fork / L 3 + S 1 C 2 )
k 23 = (C 1 S 2 H fork / L 3 + C 1 C 2 )
k 33 = (-C 2 H fork / L 3 + S 2 )
また、穿孔装置４は、第１の回転角θ1と第５の回転角θ5、第２の回転角θ2と第４の回転角θ4の同調機能を有しているため、下記（３８）式に示すように、位置合わせの基点の移動速度d0ｘ5/dt、d0ｙ5/dt、d0ｚ5/dtは、穿孔ロッド７ａの先端部の移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtと等しくなる。
Further, since the
このような手順により、上記（３０）、（３７）、（３８）式、つまり、上記（１）式が導出される。
（動作その他）
次に、実施形態に係る穿孔支援装置１の動作について説明する。
まず、穿孔装置４のオペレータが、モード切替スイッチ１４を操作し、動作モードを第１のモードに切り替えたとする。そして、指示入力部１５を操作し、第４、第５の関節１２ｄ、１２ｅを動作させる指示入力、つまり、穿孔ロッド７ａの姿勢を変更させる入力を行ったとする。すると、コントローラ１８が、指示入力部１５が出力する第４、第５の関節１２ｄ、１２ｅの動作、つまり、回転の指示入力に基づき、第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/dt、dθ5ref/dtを算出する（図３のステップＳ１０１）。
By such a procedure, the above equations (30), (37), and (38), that is, the above equation (1) are derived.
(Operation and others)
Next, the operation of the
First, it is assumed that the operator of the
続いて、コントローラ１８が、算出した目標値dθ4ref/dt、dθ5ref/dtに基づき、上記（１）式に従って、第４、第５の関節１２ｄ、１２ｅの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtをゼロとするための、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する（図３のステップＳ１０２）。
Subsequently, the
続いて、コントローラ１８が、算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dt、及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１～第３の関節１２ａ～１２ｃの動作速度dθ1/dt、dθ2/dt、dＬ3/dtを目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtと等しくするための指令を関節駆動装置８に出力する（図３のステップＳ１０３）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１～第３の関節１２ａ～１２ｃを駆動させ、動作速度dθ1/dt、dθ2/dt、dＬ3/dtが目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtと等しくなる。それゆえ、オペレータの操作による第４、第５の関節１２ｄ、１２ｅの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtが打ち消され、穿孔ロッド７ａの先端部の位置が維持される。
Subsequently, the
同時に、コントローラ１８が、算出した第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/dt、dθ5ref/dt、及び動作量検出部１３ｄ、１３ｅの検出結果に基づき、第４、第５の関節１２ｄ、１２ｅの動作速度dθ4/dt、dθ5/dtを目標値dθ4ref/dt、dθ5ref/dtと等しくするための指令を関節駆動装置８に出力する（図３のステップＳ１０３）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第４、第５の関節１２ｄ、１２ｅを駆動させ、動作速度dθ4/dt、dθ5/dtが目標値dθ4ref/dt、dθ5ref/dtと等しくなる。それゆえ、オペレータの操作に応じて、穿孔ロッド７ａの姿勢が変更される。
そして、上記フローが繰り返し実行されることで、穿孔ロッド７ａの先端部の位置を維持しつつ、穿孔装置４のオペレータが希望する穿孔ロッド７ａの姿勢に変更することができる。そのため、穿孔ロッド７ａの位置及び姿勢をより容易に調整することができる。
At the same time, the
Then, by repeatedly executing the above flow, it is possible to change the posture of the
ここで、トンネル工事では、穿孔装置４を使用して切羽２に複数の装薬孔３を穿孔しながら掘り進めてゆくが、経済性や効率性を考慮すると、できるだけ余掘りを少なくすることが重要である。なぜならば、余掘りが多くなると、余分な量のずりと呼ばれる掘削残渣が発生するだけでなく、仕上げコンクリートの施工量も余分に必要となるからである。
したがって、経済性や効率性の高いトンネル工事のためには、予定した切羽２の位置へ計画した深さの装薬孔３を掘ることが非常に重要である。そのため、オペレータは、穿孔ロッド７ａの先端を、切羽２の計画した位置に正確に設置する必要がある。このとき、オペレータは、目視によって穿孔ロッド７ａの先端が正確に目的の箇所を指しているかを確認する必要があるが、穿孔機７の姿勢によっては目視ができない場合がある。このような場合には、穿孔装置４の各関節１２ａ～１２ｈを動作させて目視可能な位置まで穿孔機７を移動させることになるが、操作の良し悪しによっては、穿孔ロッド７ａの先端位置がずれてしまったり、挿し角が変わってしまったりということが起こり得る。また、これらの操作に余分な時間を費やすこととなり、作業工程が長くなってしまうこととなる。
Here, in tunnel construction, digging is carried out while drilling a plurality of
Therefore, for economical and efficient tunnel construction, it is very important to dig a
これに対し、実施形態に係る穿孔支援装置１によれば、穿孔ロッド７ａの位置及び姿勢を容易に調整できるため、穿孔装置４の各関節１２ａ～１２ｈを動作させて目視可能な位置まで穿孔機７を容易に移動できる。それゆえ、穿孔ロッド７ａの先端を、切羽２の計画した位置に正確に設置することができ、切羽２への正確な発破孔施工によって余掘りを少なくし、経済性や効率性高くトンネル工事を行うことができる。また、穿孔ロッド７ａの位置及び姿勢の調整に費やす時間を短縮でき、作業工程を短縮できる。また、オペレータの熟達した技術が要求されず、しかも高価なセンサー類や測定装置が必要とされない。
On the other hand, according to the
一方、穿孔装置４のオペレータが、モード切替スイッチ１４を操作し、動作モードを第２のモードに切り替えたとする。そして、指示入力部１５を操作し、第６の関節１２ｆを動作させる指示入力、つまり、穿孔ロッド７ａの第６の回転角θ6（ガイドロータリー角）を変更させる入力を行ったとする。すると、コントローラ１８が、指示入力部１５が出力する第６の関節１２ｆの動作の指示入力、つまり、回転の指示入力に基づき、第６の関節１２ｆの角速度の目標値dθ6ref/dtを算出する（図５のステップＳ２０１）。
続いて、コントローラ１８が、算出した目標値dθ6ref/dtに基づき、上記（１）式に従って、第６の関節１２ｆの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtをゼロとするための、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する（図５のステップＳ２０２）。
On the other hand, it is assumed that the operator of the
Subsequently, it is expected that the
続いて、コントローラ１８が、算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dt、及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１～第３の関節１２ａ～１２ｃの動作速度dθ1/dt、dθ2/dt、dＬ3/dtを目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtと等しくするための指令を関節駆動装置８に出力する（図５のステップＳ２０３）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１～第３の関節１２ａ～１２ｃを駆動させ、動作速度dθ1/dt、dθ2/dt、dＬ3/dtが目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtと等しくなる。それゆえ、オペレータの操作による第４、第５の関節１２ｄ、１２ｅの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtが打ち消され、穿孔ロッド７ａの先端部の位置が維持される。
Subsequently, the
同時に、算出した第６の関節１２ｆの角速度の目標値dθ6ref/dt、動作量検出部１３ｆの検出結果に基づき、第６の関節１２ｆの角速度dθ6/dtを目標値dθ6ref/dtと等しくするための指令を関節駆動装置８に出力する（図５のステップＳ２０３）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第６の関節１２ｆを駆動させ、動作速度dθ6/dtが目標値dθ6ref/dtと等しくなり、第６の回転角θ6が変更される。
そして、上記フローが繰り返し実行されることで、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータが希望する第６の回転角θ6に変更できる。その際、第７の関節１２ｇの回転角度θ7が０［rad］である場合には、穿孔ロッド７ａの姿勢も維持できる。
At the same time, based on the calculated target value dθ 6 ref / dt of the angular velocity of the sixth joint 12f and the detection result of the motion amount detection unit 13f, the angular velocity dθ 6 / dt of the sixth joint 12f is set to the target value dθ 6 ref / dt. A command for equalizing with is output to the joint drive device 8 (step S203 in FIG. 5). As a result, the
Then, by repeatedly executing the above flow, the position of the tip of the
また一方、穿孔装置４のオペレータが、モード切替スイッチ１４を操作し、動作モードを第３のモードに切り替えたとする。そして、指示入力部１５を操作し、第７の関節１２ｇを動作させる指示入力、つまり、穿孔ロッド７ａの第７の回転角θ7（ガイドダンプ角）を変更させる入力を行ったとする。すると、コントローラ１８が、指示入力部１５が出力する第７の関節１２ｇの動作の指示入力、つまり、回転の指示入力に基づき、第７の関節１２ｇの角速度の目標値dθ7ref/dtを算出する（図５のステップＳ３０１）。
続いて、コントローラ１８が、取得した第７の回転角θ7に基づき、上記（１）式に従って、第７の関節１２ｇの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtをゼロとするための、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する（図６のステップＳ３０２）。
On the other hand, it is assumed that the operator of the
Subsequently, it is expected that the
続いて、コントローラ１８が、算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dt、及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１～第３の関節１２ａ～１２ｃの動作速度dθ1/dt、dθ2/dt、dＬ3/dtを目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtと等しくするための指令を関節駆動装置８に出力する（図６のステップＳ３０３）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１～第３の関節１２ａ～１２ｃを駆動させ、動作速度dθ1/dt、dθ2/dt、dＬ3/dtが目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtと等しくなる。それゆえ、オペレータの操作による第４、第５の関節１２ｄ、１２ｅの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtが打ち消され、穿孔ロッド７ａの先端部の位置が維持される。
Subsequently, the
同時に、算出した第７の関節１２ｇの角速度の目標値dθ7ref/dt、動作量検出部１３ｇの検出結果に基づき、第７の関節１２ｇの動作速度dθ7/dtを目標値dθ7ref/dtと等しくするための指令を関節駆動装置８に出力する（図６のステップＳ３０３）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第７の関節１２ｇを駆動させ、動作速度dθ7/dtが目標値dθ7ref/dtと等しくなり、第７の回転角θ7が変更される。
そして、上記フローが繰り返し実行されることで、穿孔ロッド７ａの先端部の位置を自動的に維持しつつ、穿孔装置４のオペレータが希望する第７の回転角θ7に変更できる。
At the same time, based on the calculated target value dθ 7 ref / dt of the angular velocity of the 7th joint and the detection result of the motion amount detection unit 13g, the motion velocity dθ 7 / dt of the 7th joint 12g is set to the target value dθ 7 ref /. A command for equalizing with dt is output to the joint drive device 8 (step S303 in FIG. 6). As a result, the
Then, by repeatedly executing the above flow, the position of the tip end portion of the
また一方、穿孔装置４のオペレータが、モード切替スイッチ１４を操作し、動作モードを第４のモードに切り替えたとする。すると、コントローラ１８が、計画孔記憶部１６が記憶している切羽２の穿孔パターンから計画孔を１つ選択し、選択した計画孔の開口位置ｘref、ｙref、ｚref及び挿し角φref、ψrefを読み出す（図７のステップＳ４０１）。続いて、挿し角φnow、ψnowを算出し、算出した挿し角φnow、ψnow、及び読み出した計画孔の挿し角φref、ψrefに基づき、上記（３）式に従って第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/dt、dθ5ref/dtを算出する（図７のステップＳ４０２）。
On the other hand, it is assumed that the operator of the
続いて、コントローラ１８が、穿孔ロッド７ａの先端部の位置ｘnow、ｙnow、ｚnowを取得する（図７のステップＳ４０３）。続いて、上記（４）（５）式に従って、読み出した計画孔の開口位置（ｘref、ｙref、ｚref）と、取得した穿孔ロッド７ａの先端部の位置（ｘnow、ｙnow、ｚnow）との差（距離）をゼロとするための、穿孔ロッド７ａの先端部の移動速度の目標値、つまり目標速度Ｖrefを算出する（図７のステップＳ４０４）。
続いて、コントローラ１８が、算出した目標速度Ｖref、及び算出した第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/dt、dθ5ref/dtに基づき、上記（６）（７）式に従って、第４、第５の関節１２ｄ、１２ｅの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtをゼロとするとともに、計画孔の開口位置（ｘref、ｙref、ｚref）と穿孔ロッド７ａの先端部の位置（ｘnow、ｙnow、ｚnow）とを等しくするための、第１～第３の関節１２ａ～１２ｃの動作速度の目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtを算出する（図７のステップＳ４０５）。
Subsequently, the
Subsequently, the
続いて、コントローラ１８が、算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dt及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１、第２の関節１２ａ、１２ｂの角速度dθ1/dt、dθ2/dt、及び第３の関節１２ｃの伸縮速度dＬ3/dtを目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtと等しくするための指令を、関節駆動装置８に出力する（図７のステップＳ４０６）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１～第３の関節１２ａ～１２ｃを駆動させ、動作速度dθ1/dt、dθ2/dt、dＬ3/dtが目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtと等しくなる。それゆえ、目標値dθ4ref/dt、dθ5ref/dtによる第４、第５の関節１２ｄ、１２ｅの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtが打ち消される。
Subsequently, the
また同時に、算出した第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/dt、dθ5ref/dt、及び動作量検出部１３ｄ、１３ｅの検出結果に基づき、第４、第５の関節１２ｄ、１２ｅの角速度dθ4/dt、dθ5/dtを、目標値dθ4ref/dt、dθ5ref/dtと等しくするための指令を関節駆動装置８に出力する（図７のステップＳ４０６）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第４、第５の関節１２ｄ、１２ｅを駆動させ、動作速度dθ4/dt、dθ5/dtが目標値dθ4ref/dt、dθ5ref/dtと等しくなる。
そして、上記フローが繰り返し実行されることで、穿孔ロッド７ａの先端部の位置と姿勢とを、計画孔の開口位置と挿し角とに自動的に速やかに近づけることができる。
At the same time, based on the calculated target values dθ 4 ref / dt and dθ 5 ref / dt of the angular velocities of the 4th and
Then, by repeatedly executing the above flow, the position and posture of the tip end portion of the
また一方、穿孔装置４のオペレータが、モード切替スイッチ１４を操作して、動作モードを第５のモードに切り替えたとする。そして、指示入力部１５を操作し、第４、第５の関節１２ｄ、１２ｅを動作させる指示入力、つまり、穿孔ロッド７ａの姿勢を変更させる入力を行ったとする。すると、コントローラ１８が、指示入力部１５が出力する第４、第５の関節１２ｄ、１２ｅの動作（回転）の指示入力に基づき、第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/dt、dθ5ref/dtを算出する（図９のステップＳ５０１）。
On the other hand, it is assumed that the operator of the
続いて、コントローラ１８が、算出した第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/d、dθ5ref/dt、及び動作量検出部１３ｄ、１３ｅの検出結果に基づき、第４、第５の関節１２ｄ、１２ｅの角速度dθ4/dt、dθ5/dtを目標値dθ4ref/dt、dθ5ref/dtと等しくするための指令を関節駆動装置８に出力する（図９のステップＳ５０２）。そして、上記フローが繰り返し実行され、穿孔装置４のオペレータからの指示入力に応じて、第４、第５の回転角θ4、θ5、つまり、穿孔ロッド７ａの姿勢（挿し角）を変更できる。
Subsequently, the
穿孔ロッド７ａの姿勢（挿し角）が定まった後、穿孔装置４のオペレータが、指示入力部１５を操作し、第１～第３の関節１２ａ～１２ｃを動作させる指示入力、つまり、穿孔ロッド７ａの先端部の位置を変更させる入力を行ったとする。すると、コントローラ１８が、指示入力部１５が出力する第１～第３の関節１２ａ～１２ｃの動作の指示入力に基づき、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する（図９のステップＳ５０１）。
After the posture (insertion angle) of the
続いて、コントローラ１８が、算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dt、及び動作量検出部１３ａ～１３ｃの検出結果に基づき、第１～第３の関節１２ａ～１２ｃの動作速度dθ1/dt、dθ2/dt、dＬ3/dtを目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtと等しくするための指令を関節駆動装置８に出力する（図９のステップＳ５０２）。そして、上記フローが繰り返し実行され、穿孔装置４のオペレータからの指示入力に応じて、第１、第２の回転角θ1、θ2、第３の伸縮量Ｌ3、つまり穿孔ロッド７ａの先端部の位置を変更できる。その際、θ1とθ5、θ2とθ4の同調機能を有しているため、定めた挿し角が維持される。
Subsequently, the
以上、本実施形態では、図２のペデステル９ａが第１の構成部品を構成する。以下同様に、図２のアウターブーム９ｂが第２の構成部品を構成する。さらに、図２のインナーブーム９ｃが第３の構成部品を構成する。また、図２の第１～第３の関節１２ａ～１２ｃが第１の関節群を構成する。さらに、第１、第２の関節１２ａ、１２ｂの角速度dθ1/dt、dθ2/dt、及び第３の関節１２ｃの伸縮速度dＬ3/dtが第１の関節群の動作速度を構成する。また、図２の第４～第８の関節１２ｄ～１２ｈのうちの１または複数の関節が第２の関節群を構成する。また、さらに、第４～第７の関節１２ｄ～１２ｇの角速度dθ4/dt～dθ7/dt、及び第８の関節１２ｈの伸縮速度dＬ8/dtが第２の関節群の動作速度を構成する。また、上記（１）式の0Ｊ8(48)がヤコビ行列を構成する。さらに、上記（１）式の0Ｊ5(13)
-1がヤコビ逆行列を構成する。
As described above, in the present embodiment, the
（１）このように、実施形態に係る穿孔支援装置１によれば、第１～第３の関節１２ａ～１２ｃ（以下、「第１の関節群」とも呼ぶ）以外の１または複数の関節（以下、「第２の関節群」とも呼ぶ）の動作速度の目標値に基づき、第２の関節群の動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtをゼロとするための、第１の関節群の動作速度の目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtを算出するようにした。そして、算出した目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtに基づき、第１の関節群それぞれの動作速度dθ1/dt、dθ2/dt、dＬ3/dtを制御するようにした。それゆえ、第２の関節群の動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度が打ち消されるように、第１の関節群を動作できる。そのため、穿孔ロッド７ａの先端部の位置及び姿勢をより容易に調整可能な穿孔支援装置を１を提供できる。
(1) As described above, according to the
（２）また、実施形態に係る穿孔支援装置１によれば、指示入力部１５で受け付けた指示入力に基づき、第２の関節群の動作速度の目標値を算出するため、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータが希望する穿孔ロッド７ａの姿勢に変更できる。
(2) Further, according to the
（３）また、実施形態に係る穿孔支援装置１によれば、第２の関節群の動作速度の目標値のベクトルに、その目標値のベクトルから穿孔ロッド７ａの先端部の移動速度のベクトルを算出するヤコビ行列0J8(48)とその移動速度のベクトルから第１の関節郡の動作速度の目標値のベクトル〔dθ1ref/dt、dθ2ref/dt、dＬ3ref/dt〕Tを算出するヤコビ逆行列0Ｊ5(13)
-1とを乗算して、第１の関節群の動作速度の目標値のベクトル〔dθ1ref/dt、dθ2ref/dt、dＬ3ref/dt〕Tを算出するようにした。それゆえ、ヤコビ逆行列0J5(15)
-1を正方行列、つまり直接に逆行列を算出可能な行列とすることができる。そのため、第１の関節群の動作速度の目標値のベクトル〔dθ1ref/dt、dθ2ref/dt、dＬ3ref/dt〕Tの算出を容易に行うことができる。
(3) Further, according to the
（４）さらに、実施形態に係る穿孔支援装置１によれば、第２の関節群の動作速度の目標値、計画孔の開口位置、及び穿孔ロッド７ａの先端部の位置に基づき、第２の関節群の動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度をゼロとするとともに、計画孔の開口位置と穿孔ロッド７ａの先端部の位置とを等しくするための、第１の関節群の動作速度の目標値を算出するようにした。それゆえ、穿孔ロッド７ａの先端部の位置と姿勢とを、計画孔の開口位置と挿し角とに自動的に近づけることができる。
(4) Further, according to the
（５）また、実施形態に係る穿孔支援装置１によれば、第２の関節群の動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度をゼロとするとともに、計画孔の開口位置と穿孔ロッド７ａの先端部の位置とを等しくするための、第１の関節群の動作速度の目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtを算出するようにした。それゆえ、第１の関節群の動作速度dθ1/dt、dθ2/dt、dＬ3/dtの制御を容易に行うことができる。
(5) Further, according to the
（６）さらに、実施形態に係る穿孔支援装置１によれば、加速域、定速域及び減速域からなる速度パターンを設定するようにした。それゆえ、穿孔ロッド７ａの先端部の移動開始時に移動速度をスローアップし、移動終了時に移動速度をスローダウンすることができるため、第１～第３の関節１２ａ～１２ｃに発生する振動を抑制することができる。
(6) Further, according to the
１…穿孔支援装置、２…切羽、３…装薬孔、４…穿孔装置、５…移動台車、５ａ…ヨーク、６…支持部材、７…穿孔機、７ａ…穿孔ロッド、７ｂ…ドリフタ、８…関節駆動装置、９…ブーム、９ａ…ペデステル、９ｂ…アウターブーム、９ｃ…インナーブーム、９ｄ…フォーク、９ｅ…チルトボディ、９ｆ…スイングボディ、９ｇ…ロータリーボディ、９ｈ…腕部、９ｉ…ガイドマウンチング、１０…ガイドシェル、１１ａ…第１の回転軸、１１ｂ…第２の回転軸、１１ｃ…第３の直動軸、１１ｄ…第４の回転軸、１１ｅ…第５の回転軸、１１ｆ…第６の回転軸、１１ｇ…第７の回転軸、１１ｈ…第８の直動軸、１２ａ…第１の関節、１２ｂ…第２の関節、１２ｃ…第３の関節、１２ｄ…第４の関節、１２ｅ…第５の関節、１２ｆ…第６の関節、１２ｇ…第７の関節、１２ｈ…第８の関節、１３ａ～１３ｈ…動作量検出部、１４…モード切替スイッチ、１５…指示入力部、１６…計画孔記憶部、１７…モニター、１８…コントローラ、１８ａ…第１の目標値演算部、１８ｂ…第１の演算実行部、１８ｃ…第２の演算実行部、１８ｄ…第１の関節群動作制御部、１８ｅ…第２の目標値演算部、１８ｆ…第２の関節群動作制御部、１８ｇ…位置取得部 1 ... Drilling support device, 2 ... Face, 3 ... Charge hole, 4 ... Drilling device, 5 ... Mobile carriage, 5a ... York, 6 ... Support member, 7 ... Drilling machine, 7a ... Drilling rod, 7b ... Drifter, 8 ... joint drive, 9 ... boom, 9a ... pedestal, 9b ... outer boom, 9c ... inner boom, 9d ... fork, 9e ... tilt body, 9f ... swing body, 9g ... rotary body, 9h ... arm, 9i ... guide Mounting, 10 ... Guide shell, 11a ... First rotation axis, 11b ... Second rotation axis, 11c ... Third linear motion axis, 11d ... Fourth rotation axis, 11e ... Fifth rotation axis, 11f ... 6th rotation axis, 11g ... 7th rotation axis, 11h ... 8th linear motion axis, 12a ... 1st joint, 12b ... 2nd joint, 12c ... 3rd joint, 12d ... 4th Joint, 12e ... 5th joint, 12f ... 6th joint, 12g ... 7th joint, 12h ... 8th joint, 13a-13h ... motion amount detection unit, 14 ... mode changeover switch, 15 ... instruction input unit , 16 ... Planning hole storage unit, 17 ... Monitor, 18 ... Controller, 18a ... First target value calculation unit, 18b ... First calculation execution unit, 18c ... Second calculation execution unit, 18d ... First joint Group motion control unit, 18e ... Second target value calculation unit, 18f ... Second joint group motion control unit, 18g ... Position acquisition unit
Claims (7)
前記移動台車と前記支持部材の第１の構成部品とを相対変位可能とする第１の関節、前記第１の構成部品と前記支持部材の第２の構成部品とを相対変位可能とする第２の関節、及び前記第２の構成部品と前記支持部材の第３の構成部品とを相対変位可能とする第３の関節を含む第１の関節群の動作速度の目標値を算出する第１の目標値演算部と、
前記第１の目標値演算部で算出した前記第１の関節群の動作速度の目標値に基づき、前記第１の関節群それぞれの動作速度を制御する第１の関節群動作制御部と、
前記支持部材に形成された関節のうちの前記第１の関節群以外の１または複数の関節からなる第２の関節群の動作速度の目標値を算出する第２の目標値演算部と、
前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値に基づき、前記第２の関節群それぞれの動作速度を制御する第２の関節群動作制御部と、
オペレータからの前記第２の関節群の動作の指示入力を受け付ける指示入力部と、を備え、
前記第２の目標値演算部は、前記指示入力部で受け付けた指示入力に基づき、前記第２の関節群の動作速度の目標値を算出し、
前記第１の目標値演算部は、前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値に基づき、前記第２の関節群の動作に起因して前記穿孔ロッドの先端部に発生することが予期される移動速度をゼロとするための、前記第１の関節群の動作速度の目標値を算出することを特徴とする穿孔支援装置。 The perforation rod in a perforation device equipped with a support member attached to a mobile carriage and provided with a plurality of joints including a rotary joint and a linear motion joint, and a perforator attached to the support member and having a perforation rod. A drilling support device that assists in controlling the position and posture of the tip.
A first joint that allows relative displacement between the moving carriage and the first component of the support member, and a second joint that allows relative displacement between the first component and the second component of the support member. The first joint for calculating the target value of the operating speed of the first joint group including the third joint that allows the second component and the third component of the support member to be relatively displaceable. Target value calculation unit and
A first joint group motion control unit that controls the motion velocity of each of the first joint groups based on the target value of the motion rate of the first joint group calculated by the first target value calculation unit.
A second target value calculation unit for calculating a target value of the operating speed of a second joint group composed of one or a plurality of joints other than the first joint group among the joints formed on the support member, and a second target value calculation unit.
A second joint group motion control unit that controls the motion velocity of each of the second joint groups based on the target value of the motion rate of the second joint group calculated by the second target value calculation unit .
It is provided with an instruction input unit that receives an instruction input for the operation of the second joint group from the operator .
The second target value calculation unit calculates a target value of the operation speed of the second joint group based on the instruction input received by the instruction input unit.
The first target value calculation unit is based on the target value of the operation speed of the second joint group calculated by the second target value calculation unit, and the perforation is caused by the operation of the second joint group. A drilling support device for calculating a target value of the operating speed of the first joint group in order to make the movement speed expected to occur at the tip of the rod zero.
前記移動台車と前記支持部材の第１の構成部品とを相対変位可能とする第１の関節、前記第１の構成部品と前記支持部材の第２の構成部品とを相対変位可能とする第２の関節、及び前記第２の構成部品と前記支持部材の第３の構成部品とを相対変位可能とする第３の関節を含む第１の関節群の動作速度の目標値を算出する第１の目標値演算部と、A first joint that allows relative displacement between the moving carriage and the first component of the support member, and a second joint that allows relative displacement between the first component and the second component of the support member. The first joint for calculating the target value of the operating speed of the first joint group including the third joint that allows the second component and the third component of the support member to be relatively displaceable. Target value calculation unit and
前記第１の目標値演算部で算出した前記第１の関節群の動作速度の目標値に基づき、前記第１の関節群それぞれの動作速度を制御する第１の関節群動作制御部と、A first joint group motion control unit that controls the motion velocity of each of the first joint groups based on the target value of the motion rate of the first joint group calculated by the first target value calculation unit.
前記支持部材に形成された関節のうちの前記第１の関節群以外の１または複数の関節からなる第２の関節群の動作速度の目標値を算出する第２の目標値演算部と、A second target value calculation unit for calculating a target value of the operating speed of a second joint group composed of one or a plurality of joints other than the first joint group among the joints formed on the support member, and a second target value calculation unit.
前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値に基づき、前記第２の関節群それぞれの動作速度を制御する第２の関節群動作制御部と、A second joint group motion control unit that controls the motion velocity of each of the second joint groups based on the target value of the motion rate of the second joint group calculated by the second target value calculation unit.
穿孔が計画されている計画孔の挿し角を記憶している計画孔記憶部と、を備え、It is equipped with a planning hole storage unit that stores the insertion angle of the planned hole for which drilling is planned.
前記第２の目標値演算部は、前記計画孔記憶部が記憶している前記計画孔の挿し角と前記穿孔ロッドの姿勢とが等しくなるように前記第２の関節群の動作速度の目標値を算出し、The second target value calculation unit is a target value of the operating speed of the second joint group so that the insertion angle of the planned hole stored in the planned hole storage unit and the posture of the drilling rod are equal to each other. Is calculated,
前記第１の目標値演算部は、前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値に基づき、前記第２の関節群の動作に起因して前記穿孔ロッドの先端部に発生することが予期される移動速度をゼロとするための、前記第１の関節群の動作速度の目標値を算出することを特徴とする穿孔支援装置。The first target value calculation unit is based on the target value of the operation speed of the second joint group calculated by the second target value calculation unit, and the perforation is caused by the operation of the second joint group. A drilling support device for calculating a target value of the operating speed of the first joint group in order to make the movement speed expected to occur at the tip of the rod zero.
前記計画孔記憶部は、前記計画孔の開口位置及び挿し角を記憶しており、
前記第１の目標値演算部は、前記計画孔記憶部が記憶している前記計画孔の開口位置、前記位置取得部で取得した前記穿孔ロッドの先端部の位置、及び前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値に基づき、前記第２の関節群の動作に起因して前記穿孔ロッドの先端部に発生することが予期される移動速度をゼロとするとともに、前記計画孔の開口位置と前記穿孔ロッドの先端部の位置とを等しくするための、前記第１の関節群の動作速度の目標値を算出することを特徴とする請求項３に記載の穿孔支援装置。 A position acquisition portion for acquiring the position of the tip portion of the drilling rod is provided.
The planned hole storage unit stores the opening position and insertion angle of the planned hole.
The first target value calculation unit includes the opening position of the planned hole stored in the planned hole storage unit, the position of the tip end portion of the drilling rod acquired by the position acquisition unit, and the second target. Based on the target value of the operating speed of the second joint group calculated by the value calculation unit, the moving speed expected to occur at the tip of the drilling rod due to the operation of the second joint group is determined. 3. The third aspect of the present invention is to set the number to zero and to calculate a target value of the operating speed of the first joint group in order to make the opening position of the planned hole equal to the position of the tip end portion of the drilling rod. The drilling support device described in.
前記計画孔記憶部が記憶している前記計画孔の開口位置と前記位置取得部で取得した前記穿孔ロッドの先端部の位置との差をゼロとするための、前記穿孔ロッドの先端部の移動速度の目標値を算出する第１の演算実行部と、
前記第１の演算実行部で算出した目標値、及び前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値に基づき、前記第２の関節群の動作に起因して前記穿孔
ロッドの先端部に発生することが予期される移動速度をゼロとするとともに、前記計画孔の開口位置と前記穿孔ロッドの先端部の位置とを等しくするための、前記第１の関節群の動作速度の目標値を算出する第２の演算実行部と、を備えたことを特徴とする請求項４に記載の穿孔支援装置。 The first target value calculation unit is
Movement of the tip of the drilling rod to make the difference between the opening position of the planned hole stored in the planning hole storage section and the position of the tip of the drilling rod acquired by the position acquisition section zero. The first calculation execution unit that calculates the target value of speed,
Due to the movement of the second joint group based on the target value calculated by the first calculation execution unit and the target value of the movement speed of the second joint group calculated by the second target value calculation unit. The first movement speed expected to occur at the tip of the drilling rod is set to zero, and the opening position of the planned hole is equal to the position of the tip of the drilling rod. The drilling support device according to claim 4, further comprising a second calculation execution unit for calculating a target value of the motion speed of the joint group.
前記第３の関節は、直動関節であることを特徴とする請求項１から６のいずれか１項に記載の穿孔支援装置。 The first joint and the second joint are rotary joints.
The perforation support device according to any one of claims 1 to 6, wherein the third joint is a linear motion joint.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2017224218A JP7000132B2 (en) | 2017-11-22 | 2017-11-22 | Drilling support device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2017224218A JP7000132B2 (en) | 2017-11-22 | 2017-11-22 | Drilling support device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2019094665A JP2019094665A (en) | 2019-06-20 |
| JP7000132B2 true JP7000132B2 (en) | 2022-02-04 |
Family
ID=66971188
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2017224218A Active JP7000132B2 (en) | 2017-11-22 | 2017-11-22 | Drilling support device |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP7000132B2 (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP7453066B2 (en) | 2020-06-04 | 2024-03-19 | 古河機械金属株式会社 | Drilling support device |
| CN114061655B (en) * | 2021-10-29 | 2023-03-24 | 中国石油天然气集团有限公司 | Dynamic non-contact transmission unit test evaluation device |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2010216183A (en) | 2009-03-18 | 2010-09-30 | Shimizu Corp | Drilling machine |
| JP2015230189A (en) | 2014-06-03 | 2015-12-21 | 株式会社鴻池組 | Boring navigation apparatus |
| US20160123146A1 (en) | 2013-05-27 | 2016-05-05 | Sandvik Mining And Construction Oy | Method and control system for a mining vehicle and a mining vehicle |
| JP2017057607A (en) | 2015-09-15 | 2017-03-23 | 日立建機株式会社 | Construction machine |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS6066787U (en) * | 1983-10-11 | 1985-05-11 | マツダ株式会社 | Drilling control device |
| JPH07229385A (en) * | 1994-02-16 | 1995-08-29 | Furukawa Co Ltd | Automatic hydraulic crawler drill |
| JP2965938B2 (en) * | 1997-05-23 | 1999-10-18 | マック株式会社 | Automatic drilling system |
-
2017
- 2017-11-22 JP JP2017224218A patent/JP7000132B2/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2010216183A (en) | 2009-03-18 | 2010-09-30 | Shimizu Corp | Drilling machine |
| US20160123146A1 (en) | 2013-05-27 | 2016-05-05 | Sandvik Mining And Construction Oy | Method and control system for a mining vehicle and a mining vehicle |
| JP2015230189A (en) | 2014-06-03 | 2015-12-21 | 株式会社鴻池組 | Boring navigation apparatus |
| JP2017057607A (en) | 2015-09-15 | 2017-03-23 | 日立建機株式会社 | Construction machine |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2019094665A (en) | 2019-06-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP5037678B2 (en) | Drilling pattern orientation method in curved tunnel, rock drilling device and software product | |
| US11958193B2 (en) | Communication system for an interaction system | |
| KR102322519B1 (en) | construction machinery | |
| US20200206923A1 (en) | Dynamic path for end effector control | |
| JP5986058B2 (en) | Mining vehicle and boom moving method | |
| US8644964B2 (en) | Method and system for controlling movement of an end effector on a machine | |
| JP5869547B2 (en) | Rock drilling apparatus and feed beam orientation control method | |
| US6617544B1 (en) | Control apparatus for a three-dimensional laser working machine and three-dimensional laser working machine | |
| JP6727735B2 (en) | Control of drill head direction | |
| EP2684836A1 (en) | Direction and speed control device for telescopic and articulated hydraulic lifting equipments. | |
| EP2539539B1 (en) | Method of presenting positioning information, user interface, and rock drilling rig | |
| JP7000132B2 (en) | Drilling support device | |
| JP2019173362A (en) | Construction machine | |
| WO2020195262A1 (en) | Work machine | |
| CN112162556A (en) | Navigation method, leveling robot, storage medium, processor and navigation system | |
| US11505918B2 (en) | System for remapping a control signal for excavator arm movement to a rotatory degree of freedom of a tool | |
| Wang et al. | A control method for hydraulic manipulators in automatic emulsion filling | |
| WO2020137275A1 (en) | Work machine | |
| JP7328659B2 (en) | Shoring erection device and shoring erection method using the same | |
| RU2800704C9 (en) | Machine for geological engineering operations and method for compensating deviation of the manipulator of the said machine | |
| RU2800704C1 (en) | Machine for geological engineering operations and method for compensating deviation of the manipulator of the said machine | |
| JP2015089607A (en) | Tool tip point information correction program, tool tip point information correction device, and tool tip point information correction method | |
| JP7453066B2 (en) | Drilling support device | |
| CN109667539A (en) | A kind of boom device and the drill jumbo comprising it | |
| CN114893130B (en) | Mechanical arm drilling positioning system and method, trolley, touch screen and storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20201007 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20210825 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20210831 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20211029 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20211130 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 7000132 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |