JP2019094665A

JP2019094665A - 穿孔支援装置

Info

Publication number: JP2019094665A
Application number: JP2017224218A
Authority: JP
Inventors: 悠吉田; Yu Yoshida; 征一宮越; Seiichi Miyakoshi
Original assignee: Furukawa Co Ltd; Furukawa Rock Drill Co Ltd
Current assignee: Furukawa Co Ltd; Furukawa Rock Drill Co Ltd
Priority date: 2017-11-22
Filing date: 2017-11-22
Publication date: 2019-06-20
Anticipated expiration: 2037-11-22
Also published as: JP7000132B2

Abstract

【課題】穿孔ロッドの先端部の位置及び姿勢をより容易に調整可能な穿孔支援装置を提供すること。【解決手段】支持部材６の第４〜第７の関節１２ｄ〜１２ｇの動作速度の目標値に基づき、第４〜第７の関節１２ｄ〜１２ｇの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度をゼロとするための、支持部材６の第１〜第３の関節１２ａ〜１２ｃの動作速度の目標値を算出し、算出した目標値に基づき、第１〜第３の関節１２ａ〜１２ｃの動作速度を制御するようにした。【選択図】図２

Description

本発明は、トンネル工事に用いられる穿孔装置に適用される穿孔支援装置に関する。

従来、移動台車に取り付けられたブームと、ブームに取り付けられたガイドシェルと、ガイドシェルに取り付けられ、穿孔ロッドを有する穿孔機と、を備えた穿孔装置が知られている（例えば、特許文献１参照。）。この特許文献１に記載の穿孔装置では、ブームとガイドシェルとの位置や姿勢をセンサーで測定し、測定した位置や姿勢をモニターに表示させることで、穿孔ロッドの先端部の位置や姿勢の調整を支援するようになっている。
しかしながら、特許文献１に記載の穿孔装置では、例えば、穿孔ロッドの先端部の姿勢を調整する場合、ブーム等の関節を動作させて、穿孔ロッドの姿勢を変化させると、穿孔ロッドの先端部の位置も変化するため、オペレータに高い技術が必要とされる。

特開２０１０−２１６１８３号公報

本発明は、上記のような点に着目してなされたもので、穿孔ロッドの先端部の位置及び姿勢をより容易に調整可能な穿孔支援装置を提供することを目的とする。

上記課題を解決するために、本発明の一態様は、（ａ）移動台車に取り付けられ、回転関節及び直動関節を含む複数の関節が設けられた支持部材と、支持部材に取り付けられ、穿孔ロッドを有する穿孔機とを備えた穿孔装置における、穿孔ロッドの先端部の位置及び姿勢の制御を支援する穿孔支援装置であって、（ｂ）支持部材の第１の構成部品と移動台車とを相対変位可能とする第１の関節、第１の構成部品と支持部材の第２の構成部品とを相対変位可能とする第２の関節、及び第２の構成部品と支持部材の第３の構成部品とを相対変位可能とする第３の関節を含む第１の関節群の動作速度の目標値を算出する第１の目標値演算部と、（ｃ）第１の目標値演算部で算出した第１の関節群の動作速度の目標値に基づき、第１の関節群それぞれの動作速度を制御する第１の関節群動作制御部と、（ｄ）支持部材に形成された関節のうちの第１の関節群以外の１または複数の関節からなる第２の関節群の動作速度の目標値を算出する第２の目標値演算部と、（ｅ）第２の目標値演算部で算出した第２の関節群の動作速度の目標値に基づき、第２の関節群それぞれの動作速度を制御する第２の関節群動作制御部と、を備え、（ｆ）第１の目標値演算部は、第２の目標値演算部で算出した第２の関節群の動作速度の目標値に基づき、第２の関節群の動作に起因して穿孔ロッドの先端部に発生することが予期される移動速度をゼロとするための、第１の関節群の動作速度の目標値を算出する穿孔支援装置であることを要旨とする。

本発明によれば、第２の関節群の動作に起因して穿孔ロッドの先端部に発生することが予期される移動速度が打ち消されるように、第１の関節群を動作できる。そのため、穿孔ロッドの先端部の位置及び姿勢をより容易に調整可能な穿孔支援装置を提供できる。

実施形態に係る穿孔支援装置と穿孔装置との概略構成を表す概念図である。ブームとガイドシェルとの各部の名称を説明するための説明図であり、（ａ）は平面図であり、（ｂ）は側面図である。第１のモード実行処理を表すフローチャートである。第１の関節群の動作速度の目標値の算出方法の説明図である。第２のモード実行処理を表すフローチャートである。第３のモード実行処理を表すフローチャートである。第４のモード実行処理を表すフローチャートである。第１、第２の関節群の動作速度の目標値の算出方法の説明図である。第５のモード実行処理を表すフローチャートである。数式の導出に用いるΣ₀〜Σ₈座標系を説明するための説明図であり、（ａ）は平面図であり、（ｂ）は側面図である。

本発明の実施形態に係る穿孔支援装置について図面を参照して説明する。
（構成）
図１に示すように、穿孔支援装置１は、切羽２に対し爆薬装填用の装薬孔３を穿孔するためのトンネル工事用作業用車両（以下、「穿孔装置４」とも呼ぶ）に搭載される。
穿孔装置４は、移動台車５と、移動台車５に取り付けられ、回転関節及び直動関節を含む複数の関節が設けられた支持部材６と、支持部材６に取り付けられ、穿孔ロッド７ａを有する穿孔機７と、支持部材６の各関節（回転関節、直動関節）それぞれを駆動させる関節駆動装置８と、を備えている。支持部材６は、移動台車５に取り付けられたブーム９と、ブーム９に取り付けられ、穿孔機７を搭載するガイドシェル１０と、を備えている。

図２（ａ）（ｂ）に示すように、ブーム９は、移動台車５に設けられたヨーク５ａに取り付けられたペデステル９ａと、ペデステル９ａに取り付けられた角筒状のアウターブーム９ｂと、アウターブーム９ｂに挿入された角柱状のインナーブーム９ｃと、インナーブーム９ｃに取り付けられたフォーク９ｄと、を備えている。また、フォーク９ｄに取り付けられたチルトボディ９ｅと、チルトボディ９ｅに取り付けられたスイングボディ９ｆと、スイングボディ９ｆに取り付けられたロータリーボディ９ｇと、ロータリーボディ９ｇに設けられた腕部９ｈに取り付けられたガイドマウンチング９ｉと、を更に備えている。腕部９ｈは、ロータリーボディ９ｇの回転軸と直交する方向に延びて設けられている。

ヨーク５ａとペデステル９ａとの間（連結部）には、鉛直方向（図２のＺ方向）に延びている軸を回転軸（以下、「第１の回転軸１１ａ」とも呼ぶ）とする回転関節（以下、「第１の関節１２ａ」とも呼ぶ）が形成されている。また、ペデステル９ａとアウターブーム９ｂとの間（連結部）には、移動台車５の左右方向（水平方向）に延びている軸を回転軸（以下、「第２の回転軸１１ｂ」とも呼ぶ）とする回転関節（以下、「第２の関節１２ｂ」とも呼ぶ）が形成されている。さらに、アウターブーム９ｂとインナーブーム９ｃとの連結部には、アウターブーム９ｂの長手方向に延びている直動軸（以下、「第３の直動軸１１ｃ」とも呼ぶ）に沿って伸縮する直動関節（以下、「第３の関節１２ｃ」とも呼ぶ）が形成されている。すなわち、第１の関節１２ａは移動台車５とペデステル９ａとを相対変位可能とし、第２の関節１２ｂはペデステル９ａとアウターブーム９ｂとを相対変位可能とし、第３の関節１２ｃはアウターブーム９ｂとインナーブーム９ｃとを相対変位可能とする。

なお、第１、第２の関節１２ａ、１２ｂを回転関節とし、第３の関節１２ｃを直動関節とする例を示したが、他の構成を採用することもできる。例えば、第２、第３の関節１２ｂ、１２ｃを回転関節とし、第１の関節１２ａを直動関節としてもよい。また、第１、第３の関節１２ａ、１２ｃを回転関節とし、第２の関節１２ｂを回転関節としてもよい。
フォーク９ｄとチルトボディ９ｅとの連結部には、第２の回転軸１１ｂと平行な移動台車５の左右方向に延びている軸を回転軸（以下、「第４の回転軸１１ｄ」とも呼ぶ）とする回転関節（以下、「第４の関節１２ｄ」とも呼ぶ）が形成されている。さらに、チルトボディ９ｅとスイングボディ９ｆとの連結部には、第４の回転軸１１ｄと直交する方向に延びている軸を回転軸（以下、「第５の回転軸１１ｅ」とも呼ぶ）とする回転関節（以下、「第５の関節１２ｅ」とも呼ぶ）が形成されている。図２（ａ）（ｂ）の例では、第５の回転軸１１ｅは、第１の回転軸１１ａ（鉛直方向に延びている軸）と平行となっている。

スイングボディ９ｆとロータリーボディ９ｇとの連結部には、第５の回転軸１１ｅと直交する方向に延びている軸を回転軸（以下、「第６の回転軸１１ｆ」とも呼ぶ）とする回転関節（以下、「第６の関節１２ｆ」とも呼ぶ）が形成されている。図２（ａ）（ｂ）の例では、第６の回転軸１１ｆは、第３の直動軸１１ｃと平行となっている。また、腕部９ｈとガイドマウンチング９ｉとの連結部には、腕部９ｈの方向（第６の回転軸１１ｆと直交する方向）に延びている軸を回転軸（以下、「第７の回転軸１１ｇ」とも呼ぶ）とする回転関節（以下、「第７の関節１２ｇ」とも呼ぶ）が形成されている。図２（ａ）（ｂ）の例では、第７の回転軸１１ｇは、第２、第４の回転軸１１ｂ、１１ｄと平行となっている。

ガイドシェル１０は、ガイドマウンチング９ｉに取り付けられている。ガイドマウンチング９ｉとガイドシェル１０との連結部には、第７の回転軸１１ｇと直交する方向（穿孔ロッド７ａの方向）に延びている直動軸（以下、「第８の直動軸１１ｈ」とも呼ぶ）に沿って伸縮する直動関節（以下、「第８の関節１２ｈ」とも呼ぶ）が形成されている。図２（ａ）（ｂ）の例では、第８の直動軸１１ｈは、第６の回転軸１１ｆと平行となっている。
図１に戻り、穿孔機７は、先端部に穿孔用ビットを有する穿孔ロッド７ａと、穿孔ロッド７ａの後端部に打撃を付与するドリフタ７ｂと、を備えている。穿孔ロッド７ａは、第８の直動軸１１ｈと平行な方向に延びている。そして、穿孔機７は、穿孔ロッド７ａの後端部にドリフタ７ｂで打撃を付与することで、切羽２に装薬孔３を穿孔可能となっている。

関節駆動装置８は、後述するコントローラ１８からの指令に従って、第１〜第８の関節１２ａ〜１２ｈそれぞれを駆動（回転、伸縮）させる。関節駆動装置８としては、例えば、第１〜第８の関節１２ａ〜１２ｈそれぞれに設けられた油圧シリンダ等の油圧駆動装置（不図示）への油圧供給を制御して、第１〜第８の関節１２ａ〜１２ｈそれぞれを駆動可能な油圧制御装置を採用できる。また、関節駆動装置８は、後述する第１の回転角θ₁と第５の回転角θ₅とが互いに反対方向に同じだけ回転するように、第１、第５の関節１２ａ、１２ｅを駆動させ、第２の回転角θ₂と第４の回転角θ₄とが互いに反対方向に同じだけ回転するように、第２、第４の関節１２ｂ、１２ｄを駆動させる同調機能を有している。
これにより、穿孔装置４は、コントローラ１８からの指令に応じて、第１〜第８の関節１２ａ〜１２ｈの回転や伸縮を行い、穿孔ロッド７ａの先端部の位置及び姿勢を変更可能となっている。そして、穿孔ロッド７ａの先端部を切羽２の所定位置に所定姿勢で押し当てる動作と、穿孔ロッド７ａの後端部にドリフタ７ｂで打撃を付与する動作とを繰り返すことで、切羽２の所定位置に所定角度を有する複数の装薬孔３を穿孔可能となっている。

また、穿孔支援装置１は、図１に示すように、動作量検出部１３ａ、１３ｂ、１３ｃ、１３ｄ、１３ｅ、１３ｆ、１３ｇ、１３ｈと、モード切替スイッチ１４と、指示入力部１５と、計画孔記憶部１６と、モニター１７と、コントローラ１８と、を備えている。
動作量検出部１３ａは、第１の関節１２ａによるペデステル９ａの回転角度（以下、「第１の回転角θ₁」とも呼ぶ）を検出する。また、動作量検出部１３ｂは、第２の関節１２ｂによるアウターブーム９ｂの回転角度（以下、「第２の回転角θ₂」とも呼ぶ）を検出する。さらに、動作量検出部１３ｃは、第３の関節１２ｃによる伸縮量（以下、「第３の伸縮量Ｌ₃」とも呼ぶ）を検出する。また、動作量検出部１３ｄは、第４の関節１２ｄによるチルトボディ９ｅの回転角度（以下、「第４の回転角θ₄」とも呼ぶ）を検出する。

動作量検出部１３ｅは、第５の関節１２ｅによるスイングボディ９ｆの回転角度（以下、「第５の回転角θ₅」とも呼ぶ）を検出する。また、動作量検出部１３fは、第６の関節１２ｆによるロータリーボディ９ｇの回転角度（以下、「第６の回転角θ₆」とも呼ぶ）を検出する。さらに、動作量検出部１３ｇは、第７の関節１２ｇによるガイドマウンチング９ｉの回転角度（以下、「第７の回転角θ₇」とも呼ぶ）を検出する。また、動作量検出部１３ｈは、第８の直動軸１１ｈの伸縮量（以下、「第８の伸縮量Ｌ₈」とも呼ぶ）を検出する。第１、第２、第４〜第７の回転角θ₁、θ₂、θ₄〜θ₇、及び第３、第８の伸縮量Ｌ₃、Ｌ₈の正方向は、図２に矢印で示す方向とする。そして、動作量検出部１３ａ〜１３ｈは、検出結果（θ₁、θ₂、Ｌ₃、θ₄〜θ₇、Ｌ₈）をコントローラ１８に出力する。

モード切替スイッチ１４は、オペレータからの動作モードの切り替えの指示入力を受け付ける。動作モードとしては、例えば、穿孔ロッド７ａの先端部の位置を維持しつつ第４、第５の回転角θ₄、θ₅を操作可能とする第１のモード、穿孔ロッド７ａの先端部の位置を維持しつつ第６の回転角θ₆を操作可能とする第２のモード、及び穿孔ロッド７ａの先端部の位置を維持しつつ第７の回転角θ₇を操作可能とする第３のモードを有している。すなわち、第１のモードではθ₄、θ₅への操作に対して穿孔ロッド７ａの先端部の位置を維持する補正制御を行い、第２のモードではθ₆への操作に対して補正制御を行い、第３のモードではθ₇への操作に対して補正制御を行う。また、穿孔ロッド７ａの先端部の位置や姿勢を自動制御する第４のモード、及び第１〜第４のモードのような位置や姿勢の維持の制御を行わない、つまり、手動制御とする第５のモードを更に有している。そして、モード切替スイッチ１４は、受け付けた指示入力をコントローラ１８に出力する。
なお、第１〜第３のモードの実行中は補正制御を行うθ₄〜θ₇以外の回転角等も、入力・操作可能としてもよい。また、第１〜第３のモードを同時に実行可能としてもよい。

また、第４のモードの実行中は、第２、第３のモードを実行可能としてもよい。ただし、θ₁〜θ₅は、自動制御とバッティングするため、入力しても動かすことができない。
指示入力部１５は、オペレータからの第１〜第８の関節１２ａ〜１２ｈの回転や収縮の指示入力を受け付ける。そして、受け付けた指示入力をコントローラ１８に出力する。
計画孔記憶部１６は、切羽２への穿孔パターンを記憶している。穿孔パターンとしては、例えば、穿孔が計画されている装薬孔３（以下、「計画孔」とも呼ぶ）の開口位置ｘref、ｙref、ｚref、挿し角φref、ψref、及び穿孔長からなるパターンを採用できる。
モニター１７は、オペレータから視認可能な位置に配置され、コントローラ１８で生成された画像データ（切羽２の画像と計画孔の位置のマーカーの画像データ）を表示する。

コントローラ１８は、Ａ/Ｄ(Analog to Digital)変換回路、Ｄ/Ａ(Digital to Analog)変換回路、ＣＰＵ(Central Processing Unit)、ＲＯＭ(Read Only Memory)及びＲＡＭ(Random Access Memory)等から構成した集積回路を備える。ＲＯＭは、各種処理を実現するプログラムを記憶している。ＣＰＵは、ＲＯＭが記憶しているプログラムに従って、計画孔記憶部１６が記憶している穿孔パターンに基づき、切羽２の画像とともに計画孔の位置のマーカーを表示させる指令をモニター１７に出力する。これにより、オペレータは、モニター１７を視認しながら、目的のマーカー位置に穿孔ロッド７ａの先端を移動させる。

また、ＣＰＵは、ＲＯＭが記憶しているプログラムに従って、第１の目標値演算部１８ａ（第１の演算実行部１８ｂ及び第２の演算実行部１８ｃを含む）と、第１の関節群動作制御部１８ｄと、第２の目標値演算部１８ｅと、第２の関節群動作制御部１８ｆと、位置取得部１８ｇとを実現し、これら１８ａ〜１８ｇにより、モード切替スイッチ１４が出力する指示入力に基づき、第１のモード実行処理、第２のモード実行処理、第３のモード実行処理、第４のモード実行処理、及び第５のモード実行処理のいずれかを実行する。

（第１のモード実行処理）
次に、コントローラ１８（第１、第２の目標値演算部１８ａ、１８ｅ、第１、第２の関節群動作制御部１８ｄ、１８ｆ）が実行する第１のモード実行処理について説明する。第１のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第１のモードを表している場合に、所定時間（例えば、１０［msec］）が経過するたびに実行される。
図３に示すように、まず、ステップＳ１０１では、第２の目標値演算部１８ｅは、指示入力部１５が出力する第４、第５の関節１２ｄ、１２ｅの動作の指示入力に基づき、第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ₄ref/dt、dθ₅ref/dtを算出する。

続いてステップＳ１０２に移行して、第１の目標値演算部１８ａは、ステップＳ１０１で算出した目標値dθ₄ref/dt、dθ₅ref/dtに基づき、第４、第５の関節１２ｄ、１２ｅの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtをゼロとするための、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dtを算出する。具体的には、図４に示すように、目標値dθ₄ref/dt、dθ₅ref/dtに基づき、下記（１）式に従って、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dtを算出する。その際、第６、第７の関節１２ｆ、１２ｇの角速度の目標値ｄθ₆ref/dt、ｄθ₇ref/dtはゼロとする。
〔d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dt〕^T
=⁰Ｊ₈₍₄₈₎・〔dθ₄ref/dt、dθ₅ref/dt、dθ₆ref/dt、dθ₇ref/dt〕^T
〔d⁰ｘ₅/dt、d⁰ｙ₅/dt、d⁰ｚ₅/dt〕^T
=〔d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dt〕^T
〔dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dt〕^T
=⁰Ｊ₅₍₁₃₎ ^-1・〔d⁰ｘ₅/dt、d⁰ｙ₅/dt、d⁰ｚ₅/dt〕^T …（１）
なお、上記（１）式の導出手順、及び行列⁰Ｊ₈₍₄₈₎、⁰Ｊ₅₍₁₃₎ ^-1については後述する。

続いてステップＳ１０３に移行して、第１の関節群動作制御部１８ｄは、ステップＳ１０２で算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt、第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dt、及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１、第２の関節１２ａ、１２ｂの角速度dθ₁/dt、dθ₂/dt、及び第３の関節１２ｃの伸縮速度dＬ₃/dtを目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtと等しくするための指令を関節駆動装置８に出力する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第１〜第３の関節１２ａ〜１２ｃを駆動させ、第１〜第３の関節１２ａ〜１２ｃの動作速度dθ₁/dt、dθ₂/dt、dＬ₃/dtを制御する。

また同時に、第２の関節群動作制御部１８ｆは、ステップＳ１０１で算出した第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ₄ref/dt、dθ₅ref/dt、及び動作量検出部１３ｄ、１３ｅの検出結果に基づき、第４、第５の関節１２ｄ、１２ｅの角速度dθ₄/dt、dθ₅/dtを目標値dθ₄ref/dt、dθ₅ref/dtと等しくするための指令を関節駆動装置８に出力した後、この演算処理、つまり、第１のモード実行処理を終了する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第４、第５の関節１２ｄ、１２ｅを駆動させ、第４、第５の関節１２ｄ、１２ｅの動作速度dθ₄/dt、dθ₅/dtを制御する。
これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１〜第５の関節１２ａ〜１２ｅそれぞれを駆動させることで、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、穿孔ロッド７ａの姿勢を操作可能とする。

ここで、穿孔装置４は、穿孔ロッド７ａの先端部の位置を決めるために３自由度、穿孔ロッド７ａの角度を決めるために２自由度、合計５自由度が必要となる。これに加え、操縦席からの視認性確保や、トンネル壁面との衝突回避、トンネル側壁面への穿孔を行うために、自由度に冗長性を持たせる必要があるため、合計８自由度を有している。そのため、例えば、穿孔ロッド１７ａの先端部に発生することが予期される移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtから、その移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtをゼロとするための、８自由度に対応する関節、つまり第１〜第８の関節１２ａ〜１２ｈの動作速度の目標値を行列式で算出するようにした場合、入力と出力の数が異なるため、係数行列の逆行列を直接計算できない。したがって、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、穿孔ロッド７ａの姿勢を操作可能とすることが困難である。

これに対し、実施形態に係る穿孔支援装置１によれば、第１〜第３の関節１２ａ〜１２ｃを駆動することで、他の関節の動作に起因して穿孔ロッド１７ａの先端部に発生することが予期される移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtをゼロとし、穿孔ロッド７ａの先端部の位置を維持するようにした。それゆえ、上記（１）式では、予期される移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtから、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dtを算出するようになる。そのため、式中で用いられる逆行列⁰Ｊ₅₍₁₃₎ ^-1は、３×３の正方行列となるため、直接計算できる。したがって、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、穿孔ロッド７ａの姿勢を操作可能とすることが容易となる。

（第２のモード実行処理）
次に、コントローラ１８（第１、第２の目標値演算部１８ａ、１８ｅ、第１、第２の関節群動作制御部１８ｄ、１８ｆ）が実行する第２のモード実行処理について説明する。第２のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第２のモードを表している場合に、所定時間（１０［msec］）が経過するたびに実行される。
図５に示すように、まず、ステップＳ２０１では、第２の目標値演算部１８ｅは、指示入力部１５が出力する第６の関節１２ｆの動作（回転）の指示入力に基づき、第６の関節１２ｆの角速度の目標値dθ₆ref/dtを算出する。

続いてステップＳ２０２に移行して、第１の目標値演算部１８ａは、ステップＳ２０１で算出した目標値dθ₆ref/dtに基づき、第６の関節１２ｆの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtをゼロとするための、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt及び第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dtを算出する。具体的には、目標値dθ₆ref/dtに基づき、上記（１）式に従って、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt及び第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dtを算出する。その際、第４、第５、第７の関節１２ｄ、１２ｅ、１２ｇの角速度の目標値ｄθ₄ref/dt、ｄθ₅ref/dt、ｄθ₇ref/dtはゼロとする。

続いてステップＳ２０３に移行して、第１の関節群動作制御部１８ｄは、ステップＳ２０２で算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt、第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dt、及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１、第２の関節１２ａ、１２ｂの角速度dθ₁/dt、dθ₂/dt、及び第３の関節１２ｃの伸縮速度dＬ₃/dtを目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtと等しくするための指令を関節駆動装置８に出力する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第１〜第３の関節１２ａ〜１２ｃを駆動させ、第１〜第３の関節１２ａ〜１２ｃの動作速度dθ₁/dt、dθ₂/dt、dＬ₃/dtを制御する。

また同時に、第２の関節群動作制御部１８ｆは、ステップＳ２０１で算出した第６の関節１２ｆの角速度の目標値dθ₆ref/dt、及び動作量検出部１３ｆの検出結果に基づき、第６の関節１２ｆの角速度dθ₆/dtを目標値dθ₆ref/dtと等しくするための指令を関節駆動装置８に出力した後、この演算処理、つまり、第２のモード実行処理を終了する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第６の関節１２ｆを駆動させ、第６の関節１２ｆの動作速度dθ₆/dtを制御する。
これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１〜第３、第６の関節１２ａ〜１２ｃ、１２ｆそれぞれを駆動させることで、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、第６の回転角θ₆を操作可能とする。その際、第７の関節１２ｇにおける第７の回転角θ₇が０［rad］である場合には、穿孔ロッド７ａの先端部の位置に加え、穿孔ロッド７ａの姿勢も維持される。

（第３のモード実行処理）
次に、コントローラ１８（第１、第２の目標値演算部１８ａ、１８ｅ、第１、第２の関節群動作制御部１８ｄ、１８ｆ）が実行する第３のモード実行処理について説明する。第３のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第３のモードを表している場合に、所定時間（１０［msec］）が経過するたびに実行される。
図６に示すように、まず、ステップＳ３０１では、第２の目標値演算部１８ｅは、指示入力部１５が出力する第７の関節１２ｇの動作（回転）の指示入力に基づき、第７の関節１２ｇの角速度の目標値dθ₇ref/dtを算出する。

続いてステップＳ３０２に移行して、第１の目標値演算部１８ａは、ステップＳ３０１で算出した目標値dθ₇ref/dtに基づき、第７の回転角θ₇の動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtをゼロとするための、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt及び第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dtを算出する。具体的には、目標値dθ₇ref/dtに基づき、上記（１）式に従って、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt及び第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dtを算出する。その際、第４、第５、第６の関節１２ｄ、１２ｅ、１２ｆの角速度の目標値ｄθ₄ref/dt、ｄθ₅ref/dt、ｄθ₆ref/dtはゼロとする。

続いてステップＳ３０３に移行して、第１の関節群動作制御部１８ｄは、ステップＳ３０２で算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt、第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dt、及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１、第２の関節１２ａ、１２ｂの角速度dθ₁/dt、dθ₂/dt、及び第３の関節１２ｃの伸縮速度dＬ₃/dtを目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtと等しくするための指令を関節駆動装置８に出力する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第１〜第３の関節１２ａ〜１２ｃを駆動させ、第１〜第３の関節１２ａ〜１２ｃの動作速度dθ₁/dt、dθ₂/dt、dＬ₃/dtを制御する。

また同時に、第２の関節群動作制御部１８ｆは、ステップＳ３０１で算出した第７の関節１２ｇの角速度の目標値dθ₇ref/dt、及び動作量検出部１３ｇの検出結果に基づき、第７の関節１２ｇの角速度dθ₇/dtを目標値dθ₇ref/dtと等しくするための指令を関節駆動装置８に出力した後、この演算処理、つまり、第３のモード実行処理を終了する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第７の関節１２ｇを駆動させ、第７の関節１２ｇの動作速度dθ₇/dtを制御する。
これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１〜第３、第７の関節１２ａ〜１２ｃ、１２gそれぞれを駆動させることで、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、第７の回転角θ₇を操作可能とする。なお、第７の回転角θ₇を操作する状況としては、ロックボルト穿孔がある。

（第４のモード実行処理）
次に、コントローラ１８（第１の目標値演算部１８ａ、第１、第２の演算実行部１８ｂ、１８ｃ、第１の関節群動作制御部１８ｄ、第２の目標値演算部１８ｅ、第２の関節群動作制御部１８ｆ、位置取得部１８ｇ）が実行する第４のモード実行処理について説明する。第４のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第４のモードを表している場合に、所定時間（１０［msec］）が経過するたびに実行される。
図７に示すように、まず、ステップＳ４０１では、第２の目標値演算部１８ｅは、計画孔記憶部１６が記憶している切羽２の穿孔パターンから計画孔を１つ選択する。計画孔の選択方法としては、例えば、穿孔装置４のオペレータによる指示入力部１５の操作を基に選択する方法や、予め定めた順番で自動的に選択する方法を用いることができる。続いて、選択した計画孔の開口位置ｘref、ｙref、ｚref及び挿し角φref、ψrefを読み出す。

続いてステップＳ４０２に移行して、第２の目標値演算部１８ｅは、Σ₈座標系（後述）のＹ軸方向の単位ベクトル（R₁₂、R₂₂、R₃₂）に基づき、下記（２）式に従って、切羽２の法線方向と穿孔ロッド７ａとがなす角度を挿し角φnow、ψnowとして算出する。挿し角φnowは、切羽２の法線方向と穿孔ロッド７ａとを上方から見た場合になす角度であり、挿し角ψnowは、切羽２の法線方向と穿孔ロッド７ａとを側方から見た場合になす角度である。続いて、図８に示すように、算出した挿し角φnow、ψnow、及びステップＳ４０１で読み出した計画孔の挿し角φref、ψrefに基づき、下記（３）式に従って、穿孔ロッド７ａの姿勢（挿し角φnow、ψnow）と計画孔の挿し角φref、ψrefとが等しくなるように、第４、第５の関節１２ｄの角速度の目標値dθ₄ref/dt、dθ₅ref/dtを算出する。
φnow=tan^-1(Ｒ₁₂/Ｒ₂₂)
ψnow=tan^-1(Ｒ₃₂/Ｒ₂₂) …（２）
dθ₄ref/dt=ｋψ・ψerr
dθ₅ref/dt=ｋφ・φerr …（３）
ψerr=ψref-ψnow
φerr=φref-φnow
ここで、ｋψ、ｋφは、フィードバックゲインである。

続いてステップＳ４０３に移行して、位置取得部１８ｇは、穿孔ロッド７ａの先端部の位置ｘnow、ｙnow、ｚnowを取得する。位置ｘnow、ｙnow、ｚnowを取得する方法としては、例えば、穿孔装置４とは別体に設けられ、穿孔ロッド７ａの先端部の位置を測量する測量装置（不図示）から取得する方法や、動作量検出部１３ａ〜１３ｈの検出結果（回転量、伸縮量）に基づき、穿孔ロッド７ａの先端部の位置を算出する方法を採用できる。
続いてステップＳ４０４に移行して、第１の目標値演算部１８ａ（第１の演算実行部１８ｂ）は、ステップＳ４０１で読み出した計画孔の開口位置（ｘref、ｙref、ｚref）と、ステップＳ４０３で取得した穿孔ロッド７ａの先端部の位置（ｘnow、ｙnow、ｚnow）との差（距離）をゼロとするための、穿孔ロッド７ａの先端部の移動速度の目標値（以下、「目標速度Ｖref（=〔d⁰ｘ₈ref/dt、d⁰ｙ₈ref/dt、d⁰ｚ₈ref/dt〕^T）」とも呼ぶ）を算出する。

具体的には、第１の目標値演算部１８ａ（第１の演算実行部１８ｂ）は、まず下記（４）式に従って正規化した位置誤差ベクトルＱerrを算出する。続いて、算出した正規化した位置誤差ベクトルＱerrに基づき、下記（５）式に従って目標速度Ｖrefを算出する。
Ｑerr=Ｐerr/Ｄerr …（４）
Ｐerr=〔ｘerr、ｙerr、ｚerr〕^T
Ｄerr=〔ｘerr²+ｙerr²+ｚerr²〕^1/2
ｘerr=ｘref-ｘnow
ｙerr=ｙref-ｙnow
ｚerr=ｚref-ｚnow
Ｖref=Ｑerr・Ｓref …（５）

ここで、Ｓrefは、穿孔ロッド７ａの先端部の移動速度の目標値の大きさを設定するためのスカラ値である。スカラ値Ｓrefとしては、例えば、加速域、定速域及び減速域からなる速度パターンを設定するようにしてもよい。加速域では、第１の関節群動作制御部１８ｄによる制御の開始直後に、穿孔ロッド７ａの先端部の移動速度の目標値d⁰ｘ₈ref/dt、d⁰ｙ₈ref/dt、d⁰ｚ₈ref/dtの時間変化率を徐々に増大させ、定速域では、目標値d⁰ｘ₈ref/dt、d⁰ｙ₈ref/dt、d⁰ｚ₈ref/dtが予め定められた最大速度に到達すると最大速度を維持させ、減速域では、第１の関節群動作制御部１８ｄによる制御の終了直前に、目標値d⁰ｘ₈ref/dt、d⁰ｙ₈ref/dt、d⁰ｚ₈ref/dtの時間変化率を徐々に低減させる。これにより、穿孔ロッド７ａの先端部の移動開始時に移動速度をスローアップし、移動終了時に移動速度をスローダウンすることができるため、第１〜第３の関節１２ａ〜１２ｃの振動を抑制することができる。

ちなみに、穿孔装置４では、第１〜第３の関節１２ａ〜１２ｃを、電動モータ等と異なり応答速度が遅い油圧駆動装置を用いて駆動している。それゆえ、例えば、目標値d⁰ｘ₈ref/dt、d⁰ｙ₈ref/dt、d⁰ｚ₈ref/dtの時間変化率が増大し、第１〜第３の関節１２ａ〜１２ｃの動作速度の目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtが増大すると、応答遅れやオーバーシュートが発生して、第１〜第３の関節１２ａ〜１２ｃに振動が発生する可能性がある。

続いてステップＳ４０５に移行して、第１の目標値演算部１８ａ（第２の演算実行部１８ｃ）は、ステップＳ４０２で算出した目標値dθ₄ref/dt、dθ₅ref/dt、及びステップＳ４０４で算出した目標速度Ｖrefに基づき、第４、第５の関節１２ｄ、１２ｅの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtをゼロとするとともに、計画孔の開口位置（ｘref、ｙref、ｚref）と穿孔ロッド７ａの先端部の位置（ｘnow、ｙnow、ｚnow）とを等しくするための、第１〜第３の関節１２ａ〜１２ｃの動作速度の目標値〔dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dt〕^Tを算出する。

具体的には、第１の目標値演算部１８ａ（第２の演算実行部１８ｃ）は、まず、目標値dθ₄ref/dt、dθ₅ref/dtに基づき、下記（６）式に従って穿孔ロッド７ａの先端部の移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtを算出する。続いて、算出した移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dt、及び目標速度Ｖrefに基づき、下記（７）式に従って、第１〜第３の関節１２ａ〜１２ｃの動作速度の目標値〔dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dt〕^Tを算出する。
〔d⁰ｘ₈ref/dt、d⁰ｙ₈ref/dt、d⁰ｚ₈ref/dt〕^T
=⁰Ｊ₈₍₄₈₎・〔dθ₄ref/dt、dθ₅ref/dt、０、０〕^T …（６）
〔dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dt〕^T
=⁰Ｊ₅₍₁₃₎ ^-1・(Ｖref-〔d⁰ｘ₈ref/dt、d⁰ｙ₈ref/dt、d⁰ｚ₈ref/dt〕^T） …（７）

続いてステップＳ４０６に移行して、第１の関節群動作制御部１８ｄは、ステップＳ４０５で算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt、第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dt、及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１、第２の関節１２ａ、１２ｂの角速度dθ₁/dt、dθ₂/dt、及び第３の関節１２ｃの伸縮速度dＬ₃/dtを目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtと等しくするための指令を関節駆動装置８に出力する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第１〜第３の関節１２ａ〜１２ｃを駆動させ、第１〜第３の関節１２ａ〜１２ｃの動作速度dθ₁/dt、dθ₂/dt、dＬ₃/dtを制御する。

同時に、第２の関節群動作制御部１８ｆは、ステップＳ４０２で算出した第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ₄ref/dt、dθ₅ref/dt、及び動作量検出部１３ｄ、１３ｅの検出結果に基づき、第４、第５の関節１２ｄ、１２ｅの角速度dθ₄/dt、dθ₅/dtを目標値dθ₄ref/dt、dθ₅ref/dと等しくするための指令を関節駆動装置８に出力した後、この演算処理、つまり、第４のモード実行処理を終了する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第４、第５の関節１２ｄ、１２ｅを駆動させ、第４、第５の関節１２ｄ、１２ｅの動作速度dθ₄/dt、dθ₅/dtを制御する。
これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１〜第５の関節１２ａ〜１２ｅそれぞれを駆動させることで、穿孔ロッド７ａの先端部の位置と姿勢とを、計画孔の開口位置と挿し角とに自動的にゆっくりと徐々に近づけることができる。

（第５のモード実行処理）
次に、コントローラ１８（第１、第２の目標値演算部１８ａ、１８ｅ、第１、第２の関節群動作制御部１８ｄ、１８ｆ）が実行する第５のモード実行処理について説明する。第５のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第５のモードを表している場合に、所定時間（１０［msec］）が経過するたびに実行される。
図９に示すように、まず、ステップＳ５０１では、第１の目標値演算部１８ａは、指示入力部１５が出力する第１〜第３の関節１２ａ〜１２ｃの動作の指示入力に基づき、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dtを算出する。また、第２の目標値演算部１８ｅは、指示入力部１５が出力する第４〜第７の関節１２ｄ〜１２ｇの動作の指示入力に基づき、第４〜第７の関節１２ｄ〜１２ｇの角速度の目標値dθ₄ref/dt〜dθ₇ref/dtを算出する。

続いてステップＳ５０２に移行して、第１の関節群動作制御部１８ｄは、ステップＳ５０１で算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt、第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dt、及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１、第２の関節１２ａ、１２ｂの角速度dθ₁/dt、dθ₂/dt、及び第３の関節１２ｃの伸縮速度dＬ₃/dtを目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtと等しくするための指令を関節駆動装置８に出力する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第１〜第３の関節１２ａ〜１２ｃを駆動させ、第１〜第３の関節１２ａ〜１２ｃの動作速度dθ₁/dt、dθ₂/dt、dＬ₃/dtを制御する。

また同時に、第２の関節群動作制御部１８ｆは、ステップＳ５０１で算出した第４〜第７の関節１２ｄ〜１２ｇの角速度の目標値dθ₄ref/d〜dθ₇ref/dt、及び動作量検出部１３ｄ〜１３ｇの検出結果に基づき、第４〜第７の関節１２ｄ〜１２ｇの角速度dθ₄/dt〜dθ₇/dtを目標値dθ₄ref/dt〜dθ₇ref/dtと等しくするための指令を関節駆動装置８に出力した後、この演算処理、つまり、第５のモード実行処理を終了する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第４〜第７の関節１２ｄ〜１２ｇを駆動させ、第４〜第７の関節１２ｄ〜１２ｇの動作速度dθ₄/dt〜dθ₇/dtを制御する。

（演算式の導出手順）
次に、上記（１）式の導出手順について詳細に説明する。
まず、図１０（ａ）（ｂ）に示すように、Σ₀〜Σ₈座標系の原点を設定する。そして、図１０（ａ）（ｂ）に示すように、第１、第２、第４〜第７の回転角θ₁、θ₂、θ₄〜θ₇が０［rad］であるときに、Σ₀〜Σ₈座標系のＸ方向が移動台車５の右方向と一致し、Σ₀〜Σ₈座標系のＹ方向が移動台車５の前方向と一致し、Σ₀〜Σ₈座標系のＺ方向が移動台車５の上方向と一致するようにした。また、Σ₀座標系からΣ₁座標系の原点を見た場合の位置、Σ₁座標系からΣ₂座標系の原点を見た場合の位置の位置、Σ₂座標系からΣ₃座標系の原点を見た場合の位置、Σ₄座標系からΣ₅座標系の原点を見た場合の位置、Σ₅座標系からΣ₆座標系の原点を見た場合の位置、Σ₆座標系からΣ₇座標系の原点を見た場合の位置、並びにΣ₇座標系からΣ₈座標系の原点を見た場合の位置を、下記表１に示す。

すると、Σ₀座標系とΣ₁座標系との同次変換行列⁰Ｔ₁は、下記（８）式で表される。

ここで、C₁=cosθ₁、S₁=sinθ₁である。
また、Σ₁座標系とΣ₂座標系との同次変換行列¹Ｔ₂は、下記（９）式で表される。

ここで、C₂=cosθ₂、S₂=sinθ₂である。
また、Σ₂座標系とΣ₃座標系との同次変換行列²Ｔ₃は、下記（１０）式で表される。

また、Σ₃座標系とΣ₄座標系との同次変換行列³Ｔ₄は、下記（１１）式で表される。

ここで、C₄=cosθ₄、S₄=sinθ₄である。
また、Σ₄座標系とΣ₅座標系との同次変換行列⁴Ｔ₅は、下記（１２）式で表される。

ここで、C₅=cosθ₅、S₅=sinθ₅である。
また、Σ₅座標系とΣ₆座標系との同次変換行列⁵Ｔ₆は、下記（１３）式で表される。

ここで、C₆=cosθ₆、S₆=sinθ₆である。
また、Σ₆座標系とΣ₇座標系との同次変換行列⁶Ｔ₇は、下記（１４）式で表される。

ここで、C₇=cosθ₇、S₇=sinθ₇である。
また、Σ₇座標系とΣ₈座標系との同次変換行列⁷Ｔ₈は、下記（１５）式で表される。

また、Σ₀座標系とΣ₈座標系との同次変換行列⁰Ｔ₈は、下記（１６）式で表される。

以上より、Σ₀座標系から見たΣ₈座標系の原点の座標は下記（１７）式で表される。

⁰ｘ₈=C₁(C₅(S₆(Ｌ₈S₇+C₇H_shell+H_dump)-D_rote-C₆D_dump)-S₅((-H_shellS₇)+L_rote+C₇Ｌ₈))
-S₁(
(-S₂(
S₄(S₅(S₆(Ｌ₈S₇+C₇H_shell+H_dump)-D_rote-C₆D_dump)+C₅((-H_shellS₇)+L_rote+C₇Ｌ₈)+ L_tilt)
+ C₄(C₆(Ｌ₈S₇+C₇H_shell+H_dump)+D_dumpS₆+H_tilt)-H_fork
))
+C₂(
C₄(S₅(S₆(Ｌ₈S₇+C₇H_shell+H_dump)-D_rote-C₆D_dump)+C₅((-H_shellS₇)+L_rote+C₇Ｌ₈)+L_tilt)
-S₄(C₆(Ｌ₈S₇+C₇H_shell+H_dump)+D_dumpS₆+H_tilt)+Ｌ₃
)+L_pede
)
⁰ｙ₈=C₁(
(-S₂(
S₄(S₅(S₆(Ｌ₈S₇+C₇H_shell+H_dump)-D_rote-C₆D_dump)+C₅((-H_shellS₇)+L_rote+C₇Ｌ₈)+L_tilt)
+C₄(C₆(Ｌ₈S₇+C₇H_shell+H_dump)+D_dumpS₆+H_tilt)-H_fork
))
+ C₂(
C₄(S₅(S₆(Ｌ₈S₇+C₇H_shell+H_dump)-D_rote-C₆D_dump)+C₅((-H_shellS₇)+L_rote+C₇Ｌ₈)+L_tilt)
-S₄(C₆(Ｌ₈S₇+C₇H_shell+H_dump)+D_dumpS₆+H_tilt)+Ｌ₃
)+L_pede
)
+S₁(C₅(S₆(Ｌ₈S₇+C₇H_shell+H_dump)-D_rote-C₆D_dump)-S₅((-H_shellS₇)+L_rote+C₇Ｌ₈))
+ L_york
⁰ｚ₈=C₂(
S₄(S₅(S₆(Ｌ₈S₇+C₇H_shell+H_dump)-D_rote-C₆D_dump)+C₅((-H_shellS₇)+L_rote+C₇Ｌ₈)+L_tilt)
+C₄(C₆(Ｌ₈S₇+C₇H_shell+H_dump)+D_dumpS₆+H_tilt)-H_fork
)
+S₂(
C₄(S₅(S₆(Ｌ₈S₇+C₇H_shell+H_dump)-D_rote-C₆D_dump)+C₅((-H_shellS₇)+L_rote+C₇Ｌ₈)+L_tilt)
-S₄(C₆(Ｌ₈S₇+C₇H_shell+H_dump)+D_dumpS₆+H_tilt)+Ｌ₃
)+H_york

ここで、各関節１２ａ〜１２ｈの状態量がθ₁、θ₂、Ｌ₃、θ₄、θ₅、θ₆、θ₇、Ｌ₈であるときに、第４〜第７の関節１２ｄ〜１２ｇを角速度dθ₄/dt、dθ₅/dt、dθ₆/dt、dθ₇/dtで回転させた場合、穿孔ロッド７ａの先端に発生する移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtは、穿孔ロッド７ａの先端部の座標（⁰x₈、⁰y₈、⁰z₈）、つまり上記（１７）式を第４〜第７の回転角θ₄〜θ₇で偏微分し、偏微分したものに、第４〜第７の関節１２ｄ〜１２ｇの角速度dθ₄/dt、dθ₅/dt、dθ₆/dt、dθ₇/dtを乗算することで得られる。

すなわち、チルトボディ９ｅを第４の関節１２ｄ回りに角速度dθ₄/dt で回転させたときに、穿孔ロッド７ａの先端に発生する移動速度のＸ方向成分d⁰ｘ₈/dt、Ｙ方向成分d⁰ｙ₈/dt、Ｚ方向成分d⁰ｚ₈/dtは、下記（１８）〜（２０）式で表される。
d⁰ｘ₈/dt=〔-S₁(C₂(-S₄P_4A-C₄P_4B)-S₂(C₄P_4A-S₄P_4B))〕・dθ₄/dt …（１８）
d⁰ｙ₈/dt=〔C₁(C₂(-S₄P_4A-C₄P_4B)-S₂(C₄P_4A-S₄P_4B))〕・dθ₄/dt …（１９）
d⁰ｚ₈/dt=〔S₂(-S₄P_4A-C₄P_4B)+C₂(C₄P_4A-S₄P_4B)〕・dθ₄/dt …（２０）
P_4A=S₅(S₆(S₇Ｌ₈+C₇H_shell+H_dump)-C₆D_dump-D_rote+C₅(-S₇H_shell+L_rote+C₇Ｌ₈)+L_tilt、P_4B=C₆(S₇Ｌ₈+C₇H_shell+H_dump)+S₆D_dump+H_tilt

また、スイングボディ９ｆを第５の関節１２ｅ回りに角速度dθ₅/dtで回転させたときに、穿孔ロッド７ａの先端に発生する移動速度のＸ方向成分d⁰ｘ₈/dt、Ｙ方向成分d⁰ｙ₈/dt、Ｚ方向成分d⁰ｚ₈/dtは、下記（２１）〜（２３）式で表される。
d⁰ｘ₈/dt=〔C₁ P_5B -S₁(C₂ C₄P_5A -S₂ S₄ P_5A)〕・dθ₅/dt …（２１）
d⁰ｙ₈/dt=〔S₁ P_5B +C₁(C₂ C₄P_5A -S₂ S₄ P_5A)〕・dθ₅/dt …（２２）
d⁰ｚ₈/dt=〔C₂ S₄ P_5A + S₂ C₄P_5A〕・dθ₅/dt …（２３）
P_5A= C₅ (S₆ ( S₇Ｌ₈+ C₇ H_shell + H_dump) - C₆ D_dump- D_rote)- S₅ (-S₇ H_shell + L_rote+ C₇Ｌ₈)、P_5B= - S₅ (S₆ ( S₇Ｌ₈ + C₇ H_shell+ H_dump) - C₆ D_dump - D_rote)- C₅(-S₇ H_shell + L_rote + C₇Ｌ₈)

さらに、ロータリーボディ９ｇを第６の関節１２ｆ回りに角速度dθ₆/dtで回転させたときに、穿孔ロッド７ａの先端に発生する移動速度のＸ方向成分d⁰ｘ₈/dt、Ｙ方向成分d⁰ｙ₈/dt、Ｚ方向成分d⁰ｚ₈/dtは、下記（２４）〜（２６）式で表される。
d⁰ｘ₈/dt=〔C₁C₅P_6A-S₁(C₂(C₄S₅P_6A-S₄P_6B)-S₂(C₄P_6B+S₄S₅P_6A))〕・dθ₆/dt …（２４）
d⁰ｙ₈/dt=〔C₁(C₂(C₄S₅P_6A-S₄P_6B)-S₂(C₄P_6B+S₄S₅P_6A))+C₅S₁P_6A〕・dθ₆/dt …（２５）
d⁰ｚ₈/dt=〔S₂(C₄S₅P_6A-S₄P_6B)+C₂(C₄P_6B+S₄S₅P_6A)〕・dθ₆/dt …（２６）
P_6A=C₆(S₇Ｌ₈+C₇H_shell+H_dump)+S₆D_dump、P_6B=-S₆(S₇Ｌ₈+C₇H_shell+H_dump)+C₆D_dump

また、ガイドマウンチング９ｉを第７の関節１２ｇ回りに角速度dθ₇/dtで回転させたときに、穿孔ロッド７ａの先端に発生する移動速度のＸ方向成分d⁰ｘ₈/dt、Ｙ方向成分d⁰ｙ₈/dt、Ｚ方向成分d⁰ｚ₈/dtは、下記（２７）〜（２９）式で表される。
d⁰ｘ₈/dt=〔C₁(C₅S₆P_7A-S₅P_7B)-S₁(C₂(C₄(C₅P_7B+S₅S₆P_7A)-C₆S₄P_7A)-S₂(S₄(C₅P_7B+S₅S₆P_7A)+C₄C₆P_7A))〕・dθ₇/dt …（２７）
d⁰ｙ₈/dt=〔C₁(C₂(C₄(C₅P_7B+S₅S₆P_7A)-C₆S₄P_7A)-S₂(S₄(C₅P_7B+S₅S₆P_7A)+C₄C₆P_7A))+S₁(C₅S₆P_7A-S₅P_7B)〕・dθ₇/dt …（２８）
d⁰ｚ₈/dt=〔C₂(S₄(C₅P_7B+S₅S₆P_7A)+C₄C₆P_7A)+S₂(C₄(C₅P_7B+S₅S₆P_7A)-C₆S₄P_7A)〕・dθ₇/dt …（２９）
P_7A=C₇Ｌ₈-S₇H_shell、P_7B=-S₇Ｌ₈-C₇H_shell

また、第４〜第７の関節１２ｄ〜１２ｇの動作を同時に行った場合には、上記（１８）〜（２９）式のＸ方向成分d⁰ｘ₈/dt、Ｙ方向成分d⁰ｙ₈/dt、Ｚ方向成分d⁰ｚ₈/dtを成分毎に足し合わせた速度が、穿孔ロッド７ａの先端部に発生する。それゆえ、穿孔ロッド７ａの先端部に発生する移動速度のＸ方向成分d⁰ｘ₈/dt、Ｙ方向成分d⁰ｙ₈/dt、Ｚ方向成分d⁰ｚ₈/dtは、下記（３０）式に示すように、行列式で表すことができる。

ここで、第４〜第７の関節１２ｄ〜１２ｇの角速度dθ₄/dt、dθ₅/dt、dθ₆/dt、dθ₇/dtを穿孔ロッド７ａの先端の移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtへ変換する行列⁰J₈₍₄₈₎は、ヤコビ行列と呼ばれる。ヤコビ行列⁰J₈₍₄₈₎は、各関節１２ａ〜１２ｈの状態量（回転量、伸縮量）θ₁、θ₂、Ｌ₃、θ₄、θ₅、θ₆、θ₇、Ｌ₈によって変化するため、穿孔ロッド７ａの先端部の位置や姿勢が変化した場合には更新する必要がある。なお、穿孔ロッド７ａの先端部の位置や姿勢の変化が小さい場合には、ヤコビ行列⁰J₈₍₄₈₎の変化も小さくなる。そのため、必ずしも制御周期毎にヤコビ行列⁰J₈₍₄₈₎を更新する必要はない。

また、第１〜第８の関節１２ａ〜１２ｈの状態量がθ₁、θ₂、Ｌ₃、θ₄、θ₅、θ₆、θ₇、Ｌ₈であるときに、第１〜第３の関節１２ａ〜１２ｃを角速度dθ₁/dt、dθ₂/dt、伸縮速度dＬ₃/dtで動作させた場合、位置合わせの基点に発生する移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtを算出するための数式も、上記（３０）式の導出手順と同様の手順によって得られる。ただし、穿孔装置４は、角速度dθ₂/dtに対して角速度dθ₄/dtが同調するため、角速度dθ₂/dtによって発生する基点の移動速度は、（d⁰ｘ₅/dt、d⁰ｙ₅/dt、d⁰ｚ₅/dt）=（d⁰ｘ₃/dt、d⁰ｙ₃/dt、d⁰ｚ₃/dt）となる。また、第３の関節１２ｃの伸縮速度dＬ₃/dtによって発生する基点の移動速度は、ブーム９の姿勢によらず、（d⁰ｘ₅/t、d⁰ｙ₅/t、d⁰ｚ₅/t）=（d⁰ｘ₃/dt、d⁰ｙ₃/dt、d⁰ｚ₃/dt）となる。
ここで、基点の座標の各成分⁰x₅、⁰y₅は、下記（３１）、（３２）式で表される。なお、⁰z₅は、θ₁の要素を持たず、θ₁で偏微分すると０となるため説明を省略する。
⁰x₅=⁰x₃+S₁(S₄(S₂L_tilt+C₂H_tilt)+C₄(-C₂L_tilt+S₂H_tilt)) …（３１）
⁰y₅=⁰y₃+C₁(-S₄(S₂L_tilt+C₂H_tilt)-C₄(-C₂L_tilt+S₂H_tilt)) …（３２）
⁰x₃=-S₁C₂Ｌ₃-S₁S₂H_fork-S₁L_pede
⁰y₃=C₁C₂Ｌ₃+C₁S₂H_fork+C₁L_pede+L_york
⁰z₃=S₂Ｌ₃-C₂H_fork+H_york

続いて、上記（３１）、（３２）式及び省略した⁰ｚ₅の数式を第１の回転角θ₁、θ₂や第３の伸縮量Ｌ₃で偏微分し、偏微分したものに、第１〜第３の関節１２ａ〜１２ｃの角速度dθ₁/dt、dθ₅/dtやdＬ₃/dを乗算すると、下記（３３）〜（３５）式が得られる。
d⁰ｘ₅/dt=〔(-C₁S₂H_fork-C₁L_pede-C₁C₂Ｌ₃)+C₁(S₄(S₂L_tilt+C₂H_tilt)+C₄(-C₂L_tilt+S₂H_tilt))〕・dθ₁/dt
d⁰ｙ₅/dt=〔(-S₁S₂H_fork-S₁L_pede-S₁C₂Ｌ₃)+S₁(S₄(S₂L_tilt+C₂H_tilt)+C₄(-C₂L_tilt+S₂H_tilt))〕・dθ₁/dt
d⁰ｚ₅/dt=0 …（３３）
d⁰ｘ₅/dt=〔S₁S₂Ｌ₃-S₁C₂H_fork〕・dθ₂/dt
d⁰ｙ₅/dt=〔-C₁S₂Ｌ₃＋C₁C₂H_fork〕・dθ₂/dt
d⁰ｚ₅/dt=〔C₂Ｌ₃＋S₂H_fork〕・dθ₂/dt …（３４）
d⁰ｘ₅/dt=〔-S₁C₂〕・dＬ₃/dt
d⁰ｙ₅/dt=〔C₁C₂〕・dＬ₃/dt
d⁰ｚ₅/dt=〔S₂〕・dＬ₃/dt …（３５）

また、第１〜第３の関節１２ａ〜１２ｃの動作を同時に行った場合には、上記（３３）〜（３５）式のＸ方向成分d⁰ｘ₅/dt、Ｙ方向成分d⁰ｙ₅/dt、Ｚ方向成分d⁰ｚ₅/dtを成分毎に足し合わせた速度が、位置合わせの基点に発生する。それゆえ、基点に発生する移動速度のＸ方向成分d⁰ｘ₅/dt、Ｙ方向成分d⁰ｙ₅/dt、Ｚ方向成分d⁰ｚ₅/dtは、上記（３０）式と同様に、下記（３６）式に示すように、行列式で表すことができる。

ここで、第１〜第３の関節１２ａ〜１２ｃの角速度dθ₁/dt、dθ₂/dt、dＬ₃/dtを位置合わせの基点の移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtへ変換する行列⁰J₅₍₁₃₎もヤコビ行列と呼ばれる。また、ヤコビ行列⁰J₅₍₁₅₎は、正方行列であるため、逆行列⁰J₅₍₁₅₎ ^-1を求めることができる。ヤコビ逆行列⁰J₅₍₁₅₎ ^-1を用いることで、位置合わせの基点における速度（d⁰ｘ₅/dt、d⁰ｙ₅/dt、d⁰ｚ₅/dt）を実現するための、第１〜第３の関節１２ａ〜１２ｃの動作速度（dθ₁/dt、dθ₂/dt、dＬ₃/dt）を演算可能な下記（３７）式が導出される。

k₁₁=C₁/(S₄(S₂L_tilt+C₂H_tilt)+C₄(S₂H_tilt-C₂L_tilt)-S₂H_fork-C₂Ｌ₃-L_pede)
k₂₁=S₁/(S₄(S₂L_tilt+C₂H_tilt)+C₄(S₂H_tilt-C₂L_tilt)-S₂H_fork-C₂Ｌ₃-L_pede)
k₃₁=0
k₁₂=(S₁S₂)/Ｌ₃
k₂₂=-(C₁S₂)/Ｌ₃
k₃₂=C₂/Ｌ₃
k₁₃=-(S₁S₂H_fork/Ｌ₃+S₁C₂)
k₂₃=(C₁S₂H_fork/Ｌ₃+C₁C₂)
k₃₃=(-C₂H_fork/Ｌ₃+S₂)

また、穿孔装置４は、第１の回転角θ₁と第５の回転角θ₅、第２の回転角θ₂と第４の回転角θ₄の同調機能を有しているため、下記（３８）式に示すように、位置合わせの基点の移動速度d⁰ｘ₅/dt、d⁰ｙ₅/dt、d⁰ｚ₅/dtは、穿孔ロッド７ａの先端部の移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtと等しくなる。

このような手順により、上記（３０）、（３７）、（３８）式、つまり、上記（１）式が導出される。
（動作その他）
次に、実施形態に係る穿孔支援装置１の動作について説明する。
まず、穿孔装置４のオペレータが、モード切替スイッチ１４を操作し、動作モードを第１のモードに切り替えたとする。そして、指示入力部１５を操作し、第４、第５の関節１２ｄ、１２ｅを動作させる指示入力、つまり、穿孔ロッド７ａの姿勢を変更させる入力を行ったとする。すると、コントローラ１８が、指示入力部１５が出力する第４、第５の関節１２ｄ、１２ｅの動作、つまり、回転の指示入力に基づき、第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ₄ref/dt、dθ₅ref/dtを算出する（図３のステップＳ１０１）。

続いて、コントローラ１８が、算出した目標値dθ₄ref/dt、dθ₅ref/dtに基づき、上記（１）式に従って、第４、第５の関節１２ｄ、１２ｅの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtをゼロとするための、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dtを算出する（図３のステップＳ１０２）。

続いて、コントローラ１８が、算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt、第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dt、及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１〜第３の関節１２ａ〜１２ｃの動作速度dθ₁/dt、dθ₂/dt、dＬ₃/dtを目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtと等しくするための指令を関節駆動装置８に出力する（図３のステップＳ１０３）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１〜第３の関節１２ａ〜１２ｃを駆動させ、動作速度dθ₁/dt、dθ₂/dt、dＬ₃/dtが目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtと等しくなる。それゆえ、オペレータの操作による第４、第５の関節１２ｄ、１２ｅの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtが打ち消され、穿孔ロッド７ａの先端部の位置が維持される。

同時に、コントローラ１８が、算出した第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ₄ref/dt、dθ₅ref/dt、及び動作量検出部１３ｄ、１３ｅの検出結果に基づき、第４、第５の関節１２ｄ、１２ｅの動作速度dθ₄/dt、dθ₅/dtを目標値dθ₄ref/dt、dθ₅ref/dtと等しくするための指令を関節駆動装置８に出力する（図３のステップＳ１０３）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第４、第５の関節１２ｄ、１２ｅを駆動させ、動作速度dθ₄/dt、dθ₅/dtが目標値dθ₄ref/dt、dθ₅ref/dtと等しくなる。それゆえ、オペレータの操作に応じて、穿孔ロッド７ａの姿勢が変更される。
そして、上記フローが繰り返し実行されることで、穿孔ロッド７ａの先端部の位置を維持しつつ、穿孔装置４のオペレータが希望する穿孔ロッド７ａの姿勢に変更することができる。そのため、穿孔ロッド７ａの位置及び姿勢をより容易に調整することができる。

ここで、トンネル工事では、穿孔装置４を使用して切羽２に複数の装薬孔３を穿孔しながら掘り進めてゆくが、経済性や効率性を考慮すると、できるだけ余掘りを少なくすることが重要である。なぜならば、余掘りが多くなると、余分な量のずりと呼ばれる掘削残渣が発生するだけでなく、仕上げコンクリートの施工量も余分に必要となるからである。
したがって、経済性や効率性の高いトンネル工事のためには、予定した切羽２の位置へ計画した深さの装薬孔３を掘ることが非常に重要である。そのため、オペレータは、穿孔ロッド７ａの先端を、切羽２の計画した位置に正確に設置する必要がある。このとき、オペレータは、目視によって穿孔ロッド７ａの先端が正確に目的の箇所を指しているかを確認する必要があるが、穿孔機７の姿勢によっては目視ができない場合がある。このような場合には、穿孔装置４の各関節１２ａ〜１２ｈを動作させて目視可能な位置まで穿孔機７を移動させることになるが、操作の良し悪しによっては、穿孔ロッド７ａの先端位置がずれてしまったり、挿し角が変わってしまったりということが起こり得る。また、これらの操作に余分な時間を費やすこととなり、作業工程が長くなってしまうこととなる。

これに対し、実施形態に係る穿孔支援装置１によれば、穿孔ロッド７ａの位置及び姿勢を容易に調整できるため、穿孔装置４の各関節１２ａ〜１２ｈを動作させて目視可能な位置まで穿孔機７を容易に移動できる。それゆえ、穿孔ロッド７ａの先端を、切羽２の計画した位置に正確に設置することができ、切羽２への正確な発破孔施工によって余掘りを少なくし、経済性や効率性高くトンネル工事を行うことができる。また、穿孔ロッド７ａの位置及び姿勢の調整に費やす時間を短縮でき、作業工程を短縮できる。また、オペレータの熟達した技術が要求されず、しかも高価なセンサー類や測定装置が必要とされない。

一方、穿孔装置４のオペレータが、モード切替スイッチ１４を操作し、動作モードを第２のモードに切り替えたとする。そして、指示入力部１５を操作し、第６の関節１２ｆを動作させる指示入力、つまり、穿孔ロッド７ａの第６の回転角θ₆（ガイドロータリー角）を変更させる入力を行ったとする。すると、コントローラ１８が、指示入力部１５が出力する第６の関節１２ｆの動作の指示入力、つまり、回転の指示入力に基づき、第６の関節１２ｆの角速度の目標値dθ₆ref/dtを算出する（図５のステップＳ２０１）。
続いて、コントローラ１８が、算出した目標値dθ₆ref/dtに基づき、上記（１）式に従って、第６の関節１２ｆの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtをゼロとするための、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dtを算出する（図５のステップＳ２０２）。

続いて、コントローラ１８が、算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt及び第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dt、及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１〜第３の関節１２ａ〜１２ｃの動作速度dθ₁/dt、dθ₂/dt、dＬ₃/dtを目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtと等しくするための指令を関節駆動装置８に出力する（図５のステップＳ２０３）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１〜第３の関節１２ａ〜１２ｃを駆動させ、動作速度dθ₁/dt、dθ₂/dt、dＬ₃/dtが目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtと等しくなる。それゆえ、オペレータの操作による第４、第５の関節１２ｄ、１２ｅの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtが打ち消され、穿孔ロッド７ａの先端部の位置が維持される。

同時に、算出した第６の関節１２ｆの角速度の目標値dθ₆ref/dt、動作量検出部１３ｆの検出結果に基づき、第６の関節１２ｆの角速度dθ₆/dtを目標値dθ₆ref/dtと等しくするための指令を関節駆動装置８に出力する（図５のステップＳ２０３）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第６の関節１２ｆを駆動させ、動作速度dθ₆/dtが目標値dθ₆ref/dtと等しくなり、第６の回転角θ₆が変更される。
そして、上記フローが繰り返し実行されることで、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータが希望する第６の回転角θ₆に変更できる。その際、第７の関節１２ｇの回転角度θ₇が０［rad］である場合には、穿孔ロッド７ａの姿勢も維持できる。

また一方、穿孔装置４のオペレータが、モード切替スイッチ１４を操作し、動作モードを第３のモードに切り替えたとする。そして、指示入力部１５を操作し、第７の関節１２ｇを動作させる指示入力、つまり、穿孔ロッド７ａの第７の回転角θ₇（ガイドダンプ角）を変更させる入力を行ったとする。すると、コントローラ１８が、指示入力部１５が出力する第７の関節１２ｇの動作の指示入力、つまり、回転の指示入力に基づき、第７の関節１２ｇの角速度の目標値dθ₇ref/dtを算出する（図５のステップＳ３０１）。
続いて、コントローラ１８が、取得した第７の回転角θ₇に基づき、上記（１）式に従って、第７の関節１２ｇの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtをゼロとするための、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dtを算出する（図６のステップＳ３０２）。

続いて、コントローラ１８が、算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt及び第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dt、及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１〜第３の関節１２ａ〜１２ｃの動作速度dθ₁/dt、dθ₂/dt、dＬ₃/dtを目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtと等しくするための指令を関節駆動装置８に出力する（図６のステップＳ３０３）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１〜第３の関節１２ａ〜１２ｃを駆動させ、動作速度dθ₁/dt、dθ₂/dt、dＬ₃/dtが目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtと等しくなる。それゆえ、オペレータの操作による第４、第５の関節１２ｄ、１２ｅの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtが打ち消され、穿孔ロッド７ａの先端部の位置が維持される。

同時に、算出した第７の関節１２ｇの角速度の目標値dθ₇ref/dt、動作量検出部１３ｇの検出結果に基づき、第７の関節１２ｇの動作速度dθ₇/dtを目標値dθ₇ref/dtと等しくするための指令を関節駆動装置８に出力する（図６のステップＳ３０３）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第７の関節１２ｇを駆動させ、動作速度dθ₇/dtが目標値dθ₇ref/dtと等しくなり、第７の回転角θ₇が変更される。
そして、上記フローが繰り返し実行されることで、穿孔ロッド７ａの先端部の位置を自動的に維持しつつ、穿孔装置４のオペレータが希望する第７の回転角θ₇に変更できる。

また一方、穿孔装置４のオペレータが、モード切替スイッチ１４を操作し、動作モードを第４のモードに切り替えたとする。すると、コントローラ１８が、計画孔記憶部１６が記憶している切羽２の穿孔パターンから計画孔を１つ選択し、選択した計画孔の開口位置ｘref、ｙref、ｚref及び挿し角φref、ψrefを読み出す（図７のステップＳ４０１）。続いて、挿し角φnow、ψnowを算出し、算出した挿し角φnow、ψnow、及び読み出した計画孔の挿し角φref、ψrefに基づき、上記（３）式に従って第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ₄ref/dt、dθ₅ref/dtを算出する（図７のステップＳ４０２）。

続いて、コントローラ１８が、穿孔ロッド７ａの先端部の位置ｘnow、ｙnow、ｚnowを取得する（図７のステップＳ４０３）。続いて、上記（４）（５）式に従って、読み出した計画孔の開口位置（ｘref、ｙref、ｚref）と、取得した穿孔ロッド７ａの先端部の位置（ｘnow、ｙnow、ｚnow）との差（距離）をゼロとするための、穿孔ロッド７ａの先端部の移動速度の目標値、つまり目標速度Ｖrefを算出する（図７のステップＳ４０４）。
続いて、コントローラ１８が、算出した目標速度Ｖref、及び算出した第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ₄ref/dt、dθ₅ref/dtに基づき、上記（６）（７）式に従って、第４、第５の関節１２ｄ、１２ｅの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtをゼロとするとともに、計画孔の開口位置（ｘref、ｙref、ｚref）と穿孔ロッド７ａの先端部の位置（ｘnow、ｙnow、ｚnow）とを等しくするための、第１〜第３の関節１２ａ〜１２ｃの動作速度の目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtを算出する（図７のステップＳ４０５）。

続いて、コントローラ１８が、算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt、第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dt及び動作量検出部１３ａ、１３ｂ、１３ｃの検出結果に基づき、第１、第２の関節１２ａ、１２ｂの角速度dθ₁/dt、dθ₂/dt、及び第３の関節１２ｃの伸縮速度dＬ₃/dtを目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtと等しくするための指令を、関節駆動装置８に出力する（図７のステップＳ４０６）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１〜第３の関節１２ａ〜１２ｃを駆動させ、動作速度dθ₁/dt、dθ₂/dt、dＬ₃/dtが目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtと等しくなる。それゆえ、目標値dθ₄ref/dt、dθ₅ref/dtによる第４、第５の関節１２ｄ、１２ｅの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtが打ち消される。

また同時に、算出した第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ₄ref/dt、dθ₅ref/dt、及び動作量検出部１３ｄ、１３ｅの検出結果に基づき、第４、第５の関節１２ｄ、１２ｅの角速度dθ₄/dt、dθ₅/dtを、目標値dθ₄ref/dt、dθ₅ref/dtと等しくするための指令を関節駆動装置８に出力する（図７のステップＳ４０６）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第４、第５の関節１２ｄ、１２ｅを駆動させ、動作速度dθ₄/dt、dθ₅/dtが目標値dθ₄ref/dt、dθ₅ref/dtと等しくなる。
そして、上記フローが繰り返し実行されることで、穿孔ロッド７ａの先端部の位置と姿勢とを、計画孔の開口位置と挿し角とに自動的に速やかに近づけることができる。

また一方、穿孔装置４のオペレータが、モード切替スイッチ１４を操作して、動作モードを第５のモードに切り替えたとする。そして、指示入力部１５を操作し、第４、第５の関節１２ｄ、１２ｅを動作させる指示入力、つまり、穿孔ロッド７ａの姿勢を変更させる入力を行ったとする。すると、コントローラ１８が、指示入力部１５が出力する第４、第５の関節１２ｄ、１２ｅの動作（回転）の指示入力に基づき、第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ₄ref/dt、dθ₅ref/dtを算出する（図９のステップＳ５０１）。

続いて、コントローラ１８が、算出した第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ₄ref/d、dθ₅ref/dt、及び動作量検出部１３ｄ、１３ｅの検出結果に基づき、第４、第５の関節１２ｄ、１２ｅの角速度dθ₄/dt、dθ₅/dtを目標値dθ₄ref/dt、dθ₅ref/dtと等しくするための指令を関節駆動装置８に出力する（図９のステップＳ５０２）。そして、上記フローが繰り返し実行され、穿孔装置４のオペレータからの指示入力に応じて、第４、第５の回転角θ₄、θ₅、つまり、穿孔ロッド７ａの姿勢（挿し角）を変更できる。

穿孔ロッド７ａの姿勢（挿し角）が定まった後、穿孔装置４のオペレータが、指示入力部１５を操作し、第１〜第３の関節１２ａ〜１２ｃを動作させる指示入力、つまり、穿孔ロッド７ａの先端部の位置を変更させる入力を行ったとする。すると、コントローラ１８が、指示入力部１５が出力する第１〜第３の関節１２ａ〜１２ｃの動作の指示入力に基づき、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dtを算出する（図９のステップＳ５０１）。

続いて、コントローラ１８が、算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ₁ref/dt、dθ₂ref/dt、第３の関節１２ｃの伸縮速度の目標値dＬ₃ref/dt、及び動作量検出部１３ａ〜１３ｃの検出結果に基づき、第１〜第３の関節１２ａ〜１２ｃの動作速度dθ₁/dt、dθ₂/dt、dＬ₃/dtを目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtと等しくするための指令を関節駆動装置８に出力する（図９のステップＳ５０２）。そして、上記フローが繰り返し実行され、穿孔装置４のオペレータからの指示入力に応じて、第１、第２の回転角θ₁、θ₂、第３の伸縮量Ｌ₃、つまり穿孔ロッド７ａの先端部の位置を変更できる。その際、θ₁とθ₅、θ₂とθ₄の同調機能を有しているため、定めた挿し角が維持される。

以上、本実施形態では、図２のペデステル９ａが第１の構成部品を構成する。以下同様に、図２のアウターブーム９ｂが第２の構成部品を構成する。さらに、図２のインナーブーム９ｃが第３の構成部品を構成する。また、図２の第１〜第３の関節１２ａ〜１２ｃが第１の関節群を構成する。さらに、第１、第２の関節１２ａ、１２ｂの角速度dθ₁/dt、dθ₂/dt、及び第３の関節１２ｃの伸縮速度dＬ₃/dtが第１の関節群の動作速度を構成する。また、図２の第４〜第８の関節１２ｄ〜１２ｈのうちの１または複数の関節が第２の関節群を構成する。また、さらに、第４〜第７の関節１２ｄ〜１２ｇの角速度dθ₄/dt〜dθ₇/dt、及び第８の関節１２ｈの伸縮速度dＬ₈/dtが第２の関節群の動作速度を構成する。また、上記（１）式の⁰Ｊ₈₍₄₈₎がヤコビ行列を構成する。さらに、上記（１）式の⁰Ｊ₅₍₁₃₎ ^-1がヤコビ逆行列を構成する。

（１）このように、実施形態に係る穿孔支援装置１によれば、第１〜第３の関節１２ａ〜１２ｃ（以下、「第１の関節群」とも呼ぶ）以外の１または複数の関節（以下、「第２の関節群」とも呼ぶ）の動作速度の目標値に基づき、第２の関節群の動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d⁰ｘ₈/dt、d⁰ｙ₈/dt、d⁰ｚ₈/dtをゼロとするための、第１の関節群の動作速度の目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtを算出するようにした。そして、算出した目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtに基づき、第１の関節群それぞれの動作速度dθ₁/dt、dθ₂/dt、dＬ₃/dtを制御するようにした。それゆえ、第２の関節群の動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度が打ち消されるように、第１の関節群を動作できる。そのため、穿孔ロッド７ａの先端部の位置及び姿勢をより容易に調整可能な穿孔支援装置を１を提供できる。

（２）また、実施形態に係る穿孔支援装置１によれば、指示入力部１５で受け付けた指示入力に基づき、第２の関節群の動作速度の目標値を算出するため、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータが希望する穿孔ロッド７ａの姿勢に変更できる。

（３）また、実施形態に係る穿孔支援装置１によれば、第２の関節群の動作速度の目標値のベクトルに、その目標値のベクトルから穿孔ロッド７ａの先端部の移動速度のベクトルを算出するヤコビ行列⁰J₈₍₄₈₎とその移動速度のベクトルから第１の関節郡の動作速度の目標値のベクトル〔dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dt〕^Tを算出するヤコビ逆行列⁰Ｊ₅₍₁₃₎ ^-1とを乗算して、第１の関節群の動作速度の目標値のベクトル〔dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dt〕^Tを算出するようにした。それゆえ、ヤコビ逆行列⁰J₅₍₁₅₎ ^-1を正方行列、つまり直接に逆行列を算出可能な行列とすることができる。そのため、第１の関節群の動作速度の目標値のベクトル〔dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dt〕^Tの算出を容易に行うことができる。

（４）さらに、実施形態に係る穿孔支援装置１によれば、第２の関節群の動作速度の目標値、計画孔の開口位置、及び穿孔ロッド７ａの先端部の位置に基づき、第２の関節群の動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度をゼロとするとともに、計画孔の開口位置と穿孔ロッド７ａの先端部の位置とを等しくするための、第１の関節群の動作速度の目標値を算出するようにした。それゆえ、穿孔ロッド７ａの先端部の位置と姿勢とを、計画孔の開口位置と挿し角とに自動的に近づけることができる。

（５）また、実施形態に係る穿孔支援装置１によれば、第２の関節群の動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度をゼロとするとともに、計画孔の開口位置と穿孔ロッド７ａの先端部の位置とを等しくするための、第１の関節群の動作速度の目標値dθ₁ref/dt、dθ₂ref/dt、dＬ₃ref/dtを算出するようにした。それゆえ、第１の関節群の動作速度dθ₁/dt、dθ₂/dt、dＬ₃/dtの制御を容易に行うことができる。

（６）さらに、実施形態に係る穿孔支援装置１によれば、加速域、定速域及び減速域からなる速度パターンを設定するようにした。それゆえ、穿孔ロッド７ａの先端部の移動開始時に移動速度をスローアップし、移動終了時に移動速度をスローダウンすることができるため、第１〜第３の関節１２ａ〜１２ｃに発生する振動を抑制することができる。

１…穿孔支援装置、２…切羽、３…装薬孔、４…穿孔装置、５…移動台車、５ａ…ヨーク、６…支持部材、７…穿孔機、７ａ…穿孔ロッド、７ｂ…ドリフタ、８…関節駆動装置、９…ブーム、９ａ…ペデステル、９ｂ…アウターブーム、９ｃ…インナーブーム、９ｄ…フォーク、９ｅ…チルトボディ、９ｆ…スイングボディ、９ｇ…ロータリーボディ、９ｈ…腕部、９ｉ…ガイドマウンチング、１０…ガイドシェル、１１ａ…第１の回転軸、１１ｂ…第２の回転軸、１１ｃ…第３の直動軸、１１ｄ…第４の回転軸、１１ｅ…第５の回転軸、１１ｆ…第６の回転軸、１１ｇ…第７の回転軸、１１ｈ…第８の直動軸、１２ａ…第１の関節、１２ｂ…第２の関節、１２ｃ…第３の関節、１２ｄ…第４の関節、１２ｅ…第５の関節、１２ｆ…第６の関節、１２ｇ…第７の関節、１２ｈ…第８の関節、１３ａ〜１３ｈ…動作量検出部、１４…モード切替スイッチ、１５…指示入力部、１６…計画孔記憶部、１７…モニター、１８…コントローラ、１８ａ…第１の目標値演算部、１８ｂ…第１の演算実行部、１８ｃ…第２の演算実行部、１８ｄ…第１の関節群動作制御部、１８ｅ…第２の目標値演算部、１８ｆ…第２の関節群動作制御部、１８ｇ…位置取得部

Claims

移動台車に取り付けられ、回転関節及び直動関節を含む複数の関節が設けられた支持部材と、前記支持部材に取り付けられ、穿孔ロッドを有する穿孔機とを備えた穿孔装置における、前記穿孔ロッドの先端部の位置及び姿勢の制御を支援する穿孔支援装置であって、
前記移動台車と前記支持部材の第１の構成部品とを相対変位可能とする第１の関節、前記第１の構成部品と前記支持部材の第２の構成部品とを相対変位可能とする第２の関節、及び前記第２の構成部品と前記支持部材の第３の構成部品とを相対変位可能とする第３の関節を含む第１の関節群の動作速度の目標値を算出する第１の目標値演算部と、
前記第１の目標値演算部で算出した前記第１の関節群の動作速度の目標値に基づき、前記第１の関節群それぞれの動作速度を制御する第１の関節群動作制御部と、
前記支持部材に形成された関節のうちの前記第１の関節群以外の１または複数の関節からなる第２の関節群の動作速度の目標値を算出する第２の目標値演算部と、
前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値に基づき、前記第２の関節群それぞれの動作速度を制御する第２の関節群動作制御部と、を備え、
前記第１の目標値演算部は、前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値に基づき、前記第２の関節群の動作に起因して前記穿孔ロッドの先端部に発生することが予期される移動速度をゼロとするための、前記第１の関節群の動作速度の目標値を算出することを特徴とする穿孔支援装置。
オペレータからの前記第２の関節群の動作の指示入力を受け付ける指示入力部を備え、
前記第２の目標値演算部は、前記指示入力部で受け付けた指示入力に基づき、前記第２の関節群の動作速度の目標値を算出することを特徴とする請求項１に記載の穿孔支援装置。
前記第１の目標値演算部は、前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値のベクトルに、該目標値のベクトルから前記穿孔ロッドの先端部の移動速度のベクトルを算出するヤコビ行列と、該移動速度のベクトルから前記第１の関節郡の動作速度の目標値のベクトルを算出するヤコビ逆行列とを乗算して、前記第１の関節群の動作速度の目標値のベクトルを算出することを特徴とする請求項２に記載の穿孔支援装置。
穿孔が計画されている計画孔の開口位置及び挿し角を記憶している計画孔記憶部と、
前記穿孔ロッドの先端部の位置を取得する位置取得部と、を備え、
前記第２の目標値演算部は、前記計画孔記憶部が記憶している前記計画孔の挿し角と前記穿孔ロッドの姿勢とが等しくなるように前記第２の関節群の動作速度の目標値を算出し、
前記第１の目標値演算部は、前記計画孔記憶部が記憶している前記計画孔の開口位置、前記位置取得部で取得した前記穿孔ロッドの先端部の位置、及び前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値に基づき、前記第２の関節群の動作に起因して前記穿孔ロッドの先端部に発生することが予期される移動速度をゼロとするとともに、前記計画孔の開口位置と前記穿孔ロッドの先端部の位置とを等しくするための、前記第１の関節群の動作速度の目標値を算出することを特徴とする請求項１に記載の穿孔支援装置。
前記第１の目標値演算部は、
前記計画孔記憶部が記憶している前記計画孔の開口位置と前記位置取得部で取得した前記穿孔ロッドの先端部の位置との差をゼロとするための、前記穿孔ロッドの先端部の移動速度の目標値を算出する第１の演算実行部と、
前記第１の演算実行部で算出した目標値、及び前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値に基づき、前記第２の関節群の動作に起因して前記穿孔ロッドの先端部に発生することが予期される移動速度をゼロとするとともに、前記計画孔の開口位置と前記穿孔ロッドの先端部の位置とを等しくするための、前記第１の関節群の動作速度の目標値を算出する第２の演算実行部と、を備えたことを特徴とする請求項４に記載の穿孔支援装置。
前記第１の演算実行部は、前記穿孔ロッドの先端部の移動速度の目標値として、加速域、定速域及び減速域からなる速度パターンを設定することを特徴とする請求項５に記載の穿孔支援装置。
前記第１の関節及び前記第２の関節は、回転関節であり、
前記第３の関節は、直動関節であることを特徴とする請求項１から６のいずれか１項に記載の穿孔支援装置。