ホーム > 特許ランキング > 古河機械金属株式会社
知財求人 - 知財ポータルサイト「IP Force」
▶ 古河機械金属株式会社の特許一覧 ▶ 古河ロックドリル株式会社の特許一覧
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2021-12-27
(45)【発行日】2022-02-04
(54)【発明の名称】穿孔支援装置
(51)【国際特許分類】
E21B 15/00 20060101AFI20220128BHJP
E21B 7/02 20060101ALI20220128BHJP
E21D 9/00 20060101ALI20220128BHJP
【ＦＩ】
E21B15/00
E21B7/02
E21D9/00 C
【請求項の数】
7
(21)【出願番号】P 2017224218
(22)【出願日】2017-11-22
(65)【公開番号】P2019094665
(43)【公開日】2019-06-20
【審査請求日】2020-10-07
(73)【特許権者】
【識別番号】000165974
【氏名又は名称】古河機械金属株式会社
(73)【特許権者】
【識別番号】594149398
【氏名又は名称】古河ロックドリル株式会社
(74)【代理人】
【識別番号】100066980
【弁理士】
【氏名又は名称】森 哲也
(74)【代理人】
【識別番号】100103850
【弁理士】
【氏名又は名称】田中 秀▲てつ▼
(74)【代理人】
【識別番号】100105854
【弁理士】
【氏名又は名称】廣瀬 一
(72)【発明者】
【氏名】吉田 悠
(72)【発明者】
【氏名】宮越 征一
【審査官】松本 泰典
(56)【参考文献】
【文献】特開２０１７－０５７６０７（ＪＰ，Ａ）
【文献】米国特許出願公開第２０１６／０１２３１４６（ＵＳ，Ａ１）
【文献】特開平１０－３１７８７４（ＪＰ，Ａ）
【文献】特開２０１５－２３０１８９（ＪＰ，Ａ）
【文献】実開昭６０－０６６７８７（ＪＰ，Ｕ）
【文献】特開平０７－２２９３８５（ＪＰ，Ａ）
【文献】特開２０１０－２１６１８３（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｅ２１Ｂ ７／０２
Ｅ２１Ｂ １５／００
Ｅ２１Ｄ ９／００
(57)【特許請求の範囲】
【請求項1】
移動台車に取り付けられ、回転関節及び直動関節を含む複数の関節が設けられた支持部材と、前記支持部材に取り付けられ、穿孔ロッドを有する穿孔機とを備えた穿孔装置における、前記穿孔ロッドの先端部の位置及び姿勢の制御を支援する穿孔支援装置であって、前記移動台車と前記支持部材の第１の構成部品とを相対変位可能とする第１の関節、前記第１の構成部品と前記支持部材の第２の構成部品とを相対変位可能とする第２の関節、及び前記第２の構成部品と前記支持部材の第３の構成部品とを相対変位可能とする第３の関節を含む第１の関節群の動作速度の目標値を算出する第１の目標値演算部と、
前記第１の目標値演算部で算出した前記第１の関節群の動作速度の目標値に基づき、前記第１の関節群それぞれの動作速度を制御する第１の関節群動作制御部と、
前記支持部材に形成された関節のうちの前記第１の関節群以外の１または複数の関節からなる第２の関節群の動作速度の目標値を算出する第２の目標値演算部と、
前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値に基づき、前記第２の関節群それぞれの動作速度を制御する第２の関節群動作制御部と、
オペレータからの前記第２の関節群の動作の指示入力を受け付ける指示入力部と、を備え、
前記第２の目標値演算部は、前記指示入力部で受け付けた指示入力に基づき、前記第２の関節群の動作速度の目標値を算出し、
前記第１の目標値演算部は、前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値に基づき、前記第２の関節群の動作に起因して前記穿孔ロッドの先端部に発生することが予期される移動速度をゼロとするための、前記第１の関節群の動作速度の目標値を算出することを特徴とする穿孔支援装置。
【請求項2】
前記第１の目標値演算部は、前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値のベクトルに、該目標値のベクトルから前記穿孔ロッドの先端部の移動速度のベクトルを算出するヤコビ行列と、該移動速度のベクトルから前記第１の関節群の動作速度の目標値のベクトルを算出するヤコビ逆行列とを乗算して、前記第１の関節群の動作速度の目標値のベクトルを算出することを特徴とする請求項１に記載の穿孔支援装置。
【請求項3】
移動台車に取り付けられ、回転関節及び直動関節を含む複数の関節が設けられた支持部材と、前記支持部材に取り付けられ、穿孔ロッドを有する穿孔機とを備えた穿孔装置における、前記穿孔ロッドの先端部の位置及び姿勢の制御を支援する穿孔支援装置であって、
前記移動台車と前記支持部材の第１の構成部品とを相対変位可能とする第１の関節、前記第１の構成部品と前記支持部材の第２の構成部品とを相対変位可能とする第２の関節、及び前記第２の構成部品と前記支持部材の第３の構成部品とを相対変位可能とする第３の関節を含む第１の関節群の動作速度の目標値を算出する第１の目標値演算部と、
前記第１の目標値演算部で算出した前記第１の関節群の動作速度の目標値に基づき、前記第１の関節群それぞれの動作速度を制御する第１の関節群動作制御部と、
前記支持部材に形成された関節のうちの前記第１の関節群以外の１または複数の関節からなる第２の関節群の動作速度の目標値を算出する第２の目標値演算部と、
前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値に基づき、前記第２の関節群それぞれの動作速度を制御する第２の関節群動作制御部と、
穿孔が計画されている計画孔の挿し角を記憶している計画孔記憶部と、を備え、
前記第２の目標値演算部は、前記計画孔記憶部が記憶している前記計画孔の挿し角と前記穿孔ロッドの姿勢とが等しくなるように前記第２の関節群の動作速度の目標値を算出し、
前記第１の目標値演算部は、前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値に基づき、前記第２の関節群の動作に起因して前記穿孔ロッドの先端部に発生することが予期される移動速度をゼロとするための、前記第１の関節群の動作速度の目標値を算出することを特徴とする穿孔支援装置。
【請求項4】
前記穿孔ロッドの先端部の位置を取得する位置取得部を備え、前記計画孔記憶部は、前記計画孔の開口位置及び挿し角を記憶しており、
前記第１の目標値演算部は、前記計画孔記憶部が記憶している前記計画孔の開口位置、前記位置取得部で取得した前記穿孔ロッドの先端部の位置、及び前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値に基づき、前記第２の関節群の動作に起因して前記穿孔ロッドの先端部に発生することが予期される移動速度をゼロとするとともに、前記計画孔の開口位置と前記穿孔ロッドの先端部の位置とを等しくするための、前記第１の関節群の動作速度の目標値を算出することを特徴とする請求項３に記載の穿孔支援装置。
【請求項5】
前記第１の目標値演算部は、前記計画孔記憶部が記憶している前記計画孔の開口位置と前記位置取得部で取得した前記穿孔ロッドの先端部の位置との差をゼロとするための、前記穿孔ロッドの先端部の移動速度の目標値を算出する第１の演算実行部と、
前記第１の演算実行部で算出した目標値、及び前記第２の目標値演算部で算出した前記第２の関節群の動作速度の目標値に基づき、前記第２の関節群の動作に起因して前記穿孔
ロッドの先端部に発生することが予期される移動速度をゼロとするとともに、前記計画孔の開口位置と前記穿孔ロッドの先端部の位置とを等しくするための、前記第１の関節群の動作速度の目標値を算出する第２の演算実行部と、を備えたことを特徴とする請求項４に記載の穿孔支援装置。
【請求項6】
前記第１の演算実行部は、前記穿孔ロッドの先端部の移動速度の目標値として、加速域、定速域及び減速域からなる速度パターンを設定することを特徴とする請求項５に記載の穿孔支援装置。
【請求項7】
前記第１の関節及び前記第２の関節は、回転関節であり、前記第３の関節は、直動関節であることを特徴とする請求項１から６のいずれか１項に記載の穿孔支援装置。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、トンネル工事に用いられる穿孔装置に適用される穿孔支援装置に関する。
【背景技術】
【0002】
従来、移動台車に取り付けられたブームと、ブームに取り付けられたガイドシェルと、ガイドシェルに取り付けられ、穿孔ロッドを有する穿孔機と、を備えた穿孔装置が知られている（例えば、特許文献１参照。）。この特許文献１に記載の穿孔装置では、ブームとガイドシェルとの位置や姿勢をセンサーで測定し、測定した位置や姿勢をモニターに表示させることで、穿孔ロッドの先端部の位置や姿勢の調整を支援するようになっている。
しかしながら、特許文献１に記載の穿孔装置では、例えば、穿孔ロッドの先端部の姿勢を調整する場合、ブーム等の関節を動作させて、穿孔ロッドの姿勢を変化させると、穿孔ロッドの先端部の位置も変化するため、オペレータに高い技術が必要とされる。
しかしながら、特許文献１に記載の穿孔装置では、例えば、穿孔ロッドの先端部の姿勢を調整する場合、ブーム等の関節を動作させて、穿孔ロッドの姿勢を変化させると、穿孔ロッドの先端部の位置も変化するため、オペレータに高い技術が必要とされる。
【先行技術文献】
【特許文献】
【0003】
【文献】特開２０１０－２１６１８３号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
本発明は、上記のような点に着目してなされたもので、穿孔ロッドの先端部の位置及び姿勢をより容易に調整可能な穿孔支援装置を提供することを目的とする。
【課題を解決するための手段】
【0005】
上記課題を解決するために、本発明の一態様は、（ａ）移動台車に取り付けられ、回転関節及び直動関節を含む複数の関節が設けられた支持部材と、支持部材に取り付けられ、穿孔ロッドを有する穿孔機とを備えた穿孔装置における、穿孔ロッドの先端部の位置及び姿勢の制御を支援する穿孔支援装置であって、（ｂ）支持部材の第１の構成部品と移動台車とを相対変位可能とする第１の関節、第１の構成部品と支持部材の第２の構成部品とを相対変位可能とする第２の関節、及び第２の構成部品と支持部材の第３の構成部品とを相対変位可能とする第３の関節を含む第１の関節群の動作速度の目標値を算出する第１の目標値演算部と、（ｃ）第１の目標値演算部で算出した第１の関節群の動作速度の目標値に基づき、第１の関節群それぞれの動作速度を制御する第１の関節群動作制御部と、（ｄ）支持部材に形成された関節のうちの第１の関節群以外の１または複数の関節からなる第２の関節群の動作速度の目標値を算出する第２の目標値演算部と、（ｅ）第２の目標値演算部で算出した第２の関節群の動作速度の目標値に基づき、第２の関節群それぞれの動作速度を制御する第２の関節群動作制御部と、（ｆ）オペレータからの第２の関節群の動作の指示入力を受け付ける指示入力部と、を備え、（ｇ）第２の目標値演算部は、指示入力部で受け付けた指示入力に基づき、第２の関節群の動作速度の目標値を算出し、（ｈ）第１の目標値演算部は、第２の目標値演算部で算出した第２の関節群の動作速度の目標値に基づき、第２の関節群の動作に起因して穿孔ロッドの先端部に発生することが予期される移動速度をゼロとするための、第１の関節群の動作速度の目標値を算出する穿孔支援装置であることを要旨とする。
【発明の効果】
【0006】
本発明によれば、第２の関節群の動作に起因して穿孔ロッドの先端部に発生することが予期される移動速度が打ち消されるように、第１の関節群を動作できる。そのため、穿孔ロッドの先端部の位置及び姿勢をより容易に調整可能な穿孔支援装置を提供できる。
【図面の簡単な説明】
【0007】
【図1】実施形態に係る穿孔支援装置と穿孔装置との概略構成を表す概念図である。
【図2】ブームとガイドシェルとの各部の名称を説明するための説明図であり、（ａ）は平面図であり、（ｂ）は側面図である。
【図3】第１のモード実行処理を表すフローチャートである。
【図4】第１の関節群の動作速度の目標値の算出方法の説明図である。
【図5】第２のモード実行処理を表すフローチャートである。
【図6】第３のモード実行処理を表すフローチャートである。
【図7】第４のモード実行処理を表すフローチャートである。
【図8】第１、第２の関節群の動作速度の目標値の算出方法の説明図である。
【図9】第５のモード実行処理を表すフローチャートである。
【図10】数式の導出に用いるΣ0～Σ8座標系を説明するための説明図であり、（ａ）は平面図であり、（ｂ）は側面図である。
【発明を実施するための形態】
【0008】
本発明の実施形態に係る穿孔支援装置について図面を参照して説明する。
（構成）
図１に示すように、穿孔支援装置１は、切羽２に対し爆薬装填用の装薬孔３を穿孔するためのトンネル工事用作業用車両（以下、「穿孔装置４」とも呼ぶ）に搭載される。
穿孔装置４は、移動台車５と、移動台車５に取り付けられ、回転関節及び直動関節を含む複数の関節が設けられた支持部材６と、支持部材６に取り付けられ、穿孔ロッド７ａを有する穿孔機７と、支持部材６の各関節（回転関節、直動関節）それぞれを駆動させる関節駆動装置８と、を備えている。支持部材６は、移動台車５に取り付けられたブーム９と、ブーム９に取り付けられ、穿孔機７を搭載するガイドシェル１０と、を備えている。
（構成）
図１に示すように、穿孔支援装置１は、切羽２に対し爆薬装填用の装薬孔３を穿孔するためのトンネル工事用作業用車両（以下、「穿孔装置４」とも呼ぶ）に搭載される。
穿孔装置４は、移動台車５と、移動台車５に取り付けられ、回転関節及び直動関節を含む複数の関節が設けられた支持部材６と、支持部材６に取り付けられ、穿孔ロッド７ａを有する穿孔機７と、支持部材６の各関節（回転関節、直動関節）それぞれを駆動させる関節駆動装置８と、を備えている。支持部材６は、移動台車５に取り付けられたブーム９と、ブーム９に取り付けられ、穿孔機７を搭載するガイドシェル１０と、を備えている。
【0009】
図２（ａ）（ｂ）に示すように、ブーム９は、移動台車５に設けられたヨーク５ａに取り付けられたペデステル９ａと、ペデステル９ａに取り付けられた角筒状のアウターブーム９ｂと、アウターブーム９ｂに挿入された角柱状のインナーブーム９ｃと、インナーブーム９ｃに取り付けられたフォーク９ｄと、を備えている。また、フォーク９ｄに取り付けられたチルトボディ９ｅと、チルトボディ９ｅに取り付けられたスイングボディ９ｆと、スイングボディ９ｆに取り付けられたロータリーボディ９ｇと、ロータリーボディ９ｇに設けられた腕部９ｈに取り付けられたガイドマウンチング９ｉと、を更に備えている。腕部９ｈは、ロータリーボディ９ｇの回転軸と直交する方向に延びて設けられている。
【0010】
ヨーク５ａとペデステル９ａとの間（連結部）には、鉛直方向（図２のＺ方向）に延びている軸を回転軸（以下、「第１の回転軸１１ａ」とも呼ぶ）とする回転関節（以下、「第１の関節１２ａ」とも呼ぶ）が形成されている。また、ペデステル９ａとアウターブーム９ｂとの間（連結部）には、移動台車５の左右方向（水平方向）に延びている軸を回転軸（以下、「第２の回転軸１１ｂ」とも呼ぶ）とする回転関節（以下、「第２の関節１２ｂ」とも呼ぶ）が形成されている。さらに、アウターブーム９ｂとインナーブーム９ｃとの連結部には、アウターブーム９ｂの長手方向に延びている直動軸（以下、「第３の直動軸１１ｃ」とも呼ぶ）に沿って伸縮する直動関節（以下、「第３の関節１２ｃ」とも呼ぶ）が形成されている。すなわち、第１の関節１２ａは移動台車５とペデステル９ａとを相対変位可能とし、第２の関節１２ｂはペデステル９ａとアウターブーム９ｂとを相対変位可能とし、第３の関節１２ｃはアウターブーム９ｂとインナーブーム９ｃとを相対変位可能とする。
【0011】
なお、第１、第２の関節１２ａ、１２ｂを回転関節とし、第３の関節１２ｃを直動関節とする例を示したが、他の構成を採用することもできる。例えば、第２、第３の関節１２ｂ、１２ｃを回転関節とし、第１の関節１２ａを直動関節としてもよい。また、第１、第３の関節１２ａ、１２ｃを回転関節とし、第２の関節１２ｂを回転関節としてもよい。
フォーク９ｄとチルトボディ９ｅとの連結部には、第２の回転軸１１ｂと平行な移動台車５の左右方向に延びている軸を回転軸（以下、「第４の回転軸１１ｄ」とも呼ぶ）とする回転関節（以下、「第４の関節１２ｄ」とも呼ぶ）が形成されている。さらに、チルトボディ９ｅとスイングボディ９ｆとの連結部には、第４の回転軸１１ｄと直交する方向に延びている軸を回転軸（以下、「第５の回転軸１１ｅ」とも呼ぶ）とする回転関節（以下、「第５の関節１２ｅ」とも呼ぶ）が形成されている。図２（ａ）（ｂ）の例では、第５の回転軸１１ｅは、第１の回転軸１１ａ（鉛直方向に延びている軸）と平行となっている。
フォーク９ｄとチルトボディ９ｅとの連結部には、第２の回転軸１１ｂと平行な移動台車５の左右方向に延びている軸を回転軸（以下、「第４の回転軸１１ｄ」とも呼ぶ）とする回転関節（以下、「第４の関節１２ｄ」とも呼ぶ）が形成されている。さらに、チルトボディ９ｅとスイングボディ９ｆとの連結部には、第４の回転軸１１ｄと直交する方向に延びている軸を回転軸（以下、「第５の回転軸１１ｅ」とも呼ぶ）とする回転関節（以下、「第５の関節１２ｅ」とも呼ぶ）が形成されている。図２（ａ）（ｂ）の例では、第５の回転軸１１ｅは、第１の回転軸１１ａ（鉛直方向に延びている軸）と平行となっている。
【0012】
スイングボディ９ｆとロータリーボディ９ｇとの連結部には、第５の回転軸１１ｅと直交する方向に延びている軸を回転軸（以下、「第６の回転軸１１ｆ」とも呼ぶ）とする回転関節（以下、「第６の関節１２ｆ」とも呼ぶ）が形成されている。図２（ａ）（ｂ）の例では、第６の回転軸１１ｆは、第３の直動軸１１ｃと平行となっている。また、腕部９ｈとガイドマウンチング９ｉとの連結部には、腕部９ｈの方向（第６の回転軸１１ｆと直交する方向）に延びている軸を回転軸（以下、「第７の回転軸１１ｇ」とも呼ぶ）とする回転関節（以下、「第７の関節１２ｇ」とも呼ぶ）が形成されている。図２（ａ）（ｂ）の例では、第７の回転軸１１ｇは、第２、第４の回転軸１１ｂ、１１ｄと平行となっている。
【0013】
ガイドシェル１０は、ガイドマウンチング９ｉに取り付けられている。ガイドマウンチング９ｉとガイドシェル１０との連結部には、第７の回転軸１１ｇと直交する方向（穿孔ロッド７ａの方向）に延びている直動軸（以下、「第８の直動軸１１ｈ」とも呼ぶ）に沿って伸縮する直動関節（以下、「第８の関節１２ｈ」とも呼ぶ）が形成されている。図２（ａ）（ｂ）の例では、第８の直動軸１１ｈは、第６の回転軸１１ｆと平行となっている。
図１に戻り、穿孔機７は、先端部に穿孔用ビットを有する穿孔ロッド７ａと、穿孔ロッド７ａの後端部に打撃を付与するドリフタ７ｂと、を備えている。穿孔ロッド７ａは、第８の直動軸１１ｈと平行な方向に延びている。そして、穿孔機７は、穿孔ロッド７ａの後端部にドリフタ７ｂで打撃を付与することで、切羽２に装薬孔３を穿孔可能となっている。
図１に戻り、穿孔機７は、先端部に穿孔用ビットを有する穿孔ロッド７ａと、穿孔ロッド７ａの後端部に打撃を付与するドリフタ７ｂと、を備えている。穿孔ロッド７ａは、第８の直動軸１１ｈと平行な方向に延びている。そして、穿孔機７は、穿孔ロッド７ａの後端部にドリフタ７ｂで打撃を付与することで、切羽２に装薬孔３を穿孔可能となっている。
【0014】
関節駆動装置８は、後述するコントローラ１８からの指令に従って、第１～第８の関節１２ａ～１２ｈそれぞれを駆動（回転、伸縮）させる。関節駆動装置８としては、例えば、第１～第８の関節１２ａ～１２ｈそれぞれに設けられた油圧シリンダ等の油圧駆動装置（不図示）への油圧供給を制御して、第１～第８の関節１２ａ～１２ｈそれぞれを駆動可能な油圧制御装置を採用できる。また、関節駆動装置８は、後述する第１の回転角θ1と第５の回転角θ5とが互いに反対方向に同じだけ回転するように、第１、第５の関節１２ａ、１２ｅを駆動させ、第２の回転角θ2と第４の回転角θ4とが互いに反対方向に同じだけ回転するように、第２、第４の関節１２ｂ、１２ｄを駆動させる同調機能を有している。
これにより、穿孔装置４は、コントローラ１８からの指令に応じて、第１～第８の関節１２ａ～１２ｈの回転や伸縮を行い、穿孔ロッド７ａの先端部の位置及び姿勢を変更可能となっている。そして、穿孔ロッド７ａの先端部を切羽２の所定位置に所定姿勢で押し当てる動作と、穿孔ロッド７ａの後端部にドリフタ７ｂで打撃を付与する動作とを繰り返すことで、切羽２の所定位置に所定角度を有する複数の装薬孔３を穿孔可能となっている。
これにより、穿孔装置４は、コントローラ１８からの指令に応じて、第１～第８の関節１２ａ～１２ｈの回転や伸縮を行い、穿孔ロッド７ａの先端部の位置及び姿勢を変更可能となっている。そして、穿孔ロッド７ａの先端部を切羽２の所定位置に所定姿勢で押し当てる動作と、穿孔ロッド７ａの後端部にドリフタ７ｂで打撃を付与する動作とを繰り返すことで、切羽２の所定位置に所定角度を有する複数の装薬孔３を穿孔可能となっている。
【0015】
また、穿孔支援装置１は、図１に示すように、動作量検出部１３ａ、１３ｂ、１３ｃ、１３ｄ、１３ｅ、１３ｆ、１３ｇ、１３ｈと、モード切替スイッチ１４と、指示入力部１５と、計画孔記憶部１６と、モニター１７と、コントローラ１８と、を備えている。
動作量検出部１３ａは、第１の関節１２ａによるペデステル９ａの回転角度（以下、「第１の回転角θ1」とも呼ぶ）を検出する。また、動作量検出部１３ｂは、第２の関節１２ｂによるアウターブーム９ｂの回転角度（以下、「第２の回転角θ2」とも呼ぶ）を検出する。さらに、動作量検出部１３ｃは、第３の関節１２ｃによる伸縮量（以下、「第３の伸縮量Ｌ3」とも呼ぶ）を検出する。また、動作量検出部１３ｄは、第４の関節１２ｄによるチルトボディ９ｅの回転角度（以下、「第４の回転角θ4」とも呼ぶ）を検出する。
動作量検出部１３ａは、第１の関節１２ａによるペデステル９ａの回転角度（以下、「第１の回転角θ1」とも呼ぶ）を検出する。また、動作量検出部１３ｂは、第２の関節１２ｂによるアウターブーム９ｂの回転角度（以下、「第２の回転角θ2」とも呼ぶ）を検出する。さらに、動作量検出部１３ｃは、第３の関節１２ｃによる伸縮量（以下、「第３の伸縮量Ｌ3」とも呼ぶ）を検出する。また、動作量検出部１３ｄは、第４の関節１２ｄによるチルトボディ９ｅの回転角度（以下、「第４の回転角θ4」とも呼ぶ）を検出する。
【0016】
動作量検出部１３ｅは、第５の関節１２ｅによるスイングボディ９ｆの回転角度（以下、「第５の回転角θ5」とも呼ぶ）を検出する。また、動作量検出部１３fは、第６の関節１２ｆによるロータリーボディ９ｇの回転角度（以下、「第６の回転角θ6」とも呼ぶ）を検出する。さらに、動作量検出部１３ｇは、第７の関節１２ｇによるガイドマウンチング９ｉの回転角度（以下、「第７の回転角θ7」とも呼ぶ）を検出する。また、動作量検出部１３ｈは、第８の直動軸１１ｈの伸縮量（以下、「第８の伸縮量Ｌ8」とも呼ぶ）を検出する。第１、第２、第４～第７の回転角θ1、θ2、θ4～θ7、及び第３、第８の伸縮量Ｌ3、Ｌ8の正方向は、図２に矢印で示す方向とする。そして、動作量検出部１３ａ～１３ｈは、検出結果（θ1、θ2、Ｌ3、θ4～θ7、Ｌ8）をコントローラ１８に出力する。
【0017】
モード切替スイッチ１４は、オペレータからの動作モードの切り替えの指示入力を受け付ける。動作モードとしては、例えば、穿孔ロッド７ａの先端部の位置を維持しつつ第４、第５の回転角θ4、θ5を操作可能とする第１のモード、穿孔ロッド７ａの先端部の位置を維持しつつ第６の回転角θ6を操作可能とする第２のモード、及び穿孔ロッド７ａの先端部の位置を維持しつつ第７の回転角θ7を操作可能とする第３のモードを有している。すなわち、第１のモードではθ4、θ5への操作に対して穿孔ロッド７ａの先端部の位置を維持する補正制御を行い、第２のモードではθ6への操作に対して補正制御を行い、第３のモードではθ7への操作に対して補正制御を行う。また、穿孔ロッド７ａの先端部の位置や姿勢を自動制御する第４のモード、及び第１～第４のモードのような位置や姿勢の維持の制御を行わない、つまり、手動制御とする第５のモードを更に有している。そして、モード切替スイッチ１４は、受け付けた指示入力をコントローラ１８に出力する。
なお、第１～第３のモードの実行中は補正制御を行うθ4～θ7以外の回転角等も、入力・操作可能としてもよい。また、第１～第３のモードを同時に実行可能としてもよい。
なお、第１～第３のモードの実行中は補正制御を行うθ4～θ7以外の回転角等も、入力・操作可能としてもよい。また、第１～第３のモードを同時に実行可能としてもよい。
【0018】
また、第４のモードの実行中は、第２、第３のモードを実行可能としてもよい。ただし、θ1～θ5は、自動制御とバッティングするため、入力しても動かすことができない。
指示入力部１５は、オペレータからの第１～第８の関節１２ａ～１２ｈの回転や収縮の指示入力を受け付ける。そして、受け付けた指示入力をコントローラ１８に出力する。
計画孔記憶部１６は、切羽２への穿孔パターンを記憶している。穿孔パターンとしては、例えば、穿孔が計画されている装薬孔３（以下、「計画孔」とも呼ぶ）の開口位置ｘref、ｙref、ｚref、挿し角φref、ψref、及び穿孔長からなるパターンを採用できる。
モニター１７は、オペレータから視認可能な位置に配置され、コントローラ１８で生成された画像データ（切羽２の画像と計画孔の位置のマーカーの画像データ）を表示する。
指示入力部１５は、オペレータからの第１～第８の関節１２ａ～１２ｈの回転や収縮の指示入力を受け付ける。そして、受け付けた指示入力をコントローラ１８に出力する。
計画孔記憶部１６は、切羽２への穿孔パターンを記憶している。穿孔パターンとしては、例えば、穿孔が計画されている装薬孔３（以下、「計画孔」とも呼ぶ）の開口位置ｘref、ｙref、ｚref、挿し角φref、ψref、及び穿孔長からなるパターンを採用できる。
モニター１７は、オペレータから視認可能な位置に配置され、コントローラ１８で生成された画像データ（切羽２の画像と計画孔の位置のマーカーの画像データ）を表示する。
【0019】
コントローラ１８は、Ａ/Ｄ(Analog to Digital)変換回路、Ｄ/Ａ(Digital to Analog)変換回路、ＣＰＵ(Central Processing Unit)、ＲＯＭ(Read Only Memory)及びＲＡＭ(Random Access Memory)等から構成した集積回路を備える。ＲＯＭは、各種処理を実現するプログラムを記憶している。ＣＰＵは、ＲＯＭが記憶しているプログラムに従って、計画孔記憶部１６が記憶している穿孔パターンに基づき、切羽２の画像とともに計画孔の位置のマーカーを表示させる指令をモニター１７に出力する。これにより、オペレータは、モニター１７を視認しながら、目的のマーカー位置に穿孔ロッド７ａの先端を移動させる。
【0020】
また、ＣＰＵは、ＲＯＭが記憶しているプログラムに従って、第１の目標値演算部１８ａ（第１の演算実行部１８ｂ及び第２の演算実行部１８ｃを含む）と、第１の関節群動作制御部１８ｄと、第２の目標値演算部１８ｅと、第２の関節群動作制御部１８ｆと、位置取得部１８ｇとを実現し、これら１８ａ～１８ｇにより、モード切替スイッチ１４が出力する指示入力に基づき、第１のモード実行処理、第２のモード実行処理、第３のモード実行処理、第４のモード実行処理、及び第５のモード実行処理のいずれかを実行する。
【0021】
（第１のモード実行処理）
次に、コントローラ１８（第１、第２の目標値演算部１８ａ、１８ｅ、第１、第２の関節群動作制御部１８ｄ、１８ｆ）が実行する第１のモード実行処理について説明する。第１のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第１のモードを表している場合に、所定時間（例えば、１０［msec］）が経過するたびに実行される。
図３に示すように、まず、ステップＳ１０１では、第２の目標値演算部１８ｅは、指示入力部１５が出力する第４、第５の関節１２ｄ、１２ｅの動作の指示入力に基づき、第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/dt、dθ5ref/dtを算出する。
次に、コントローラ１８（第１、第２の目標値演算部１８ａ、１８ｅ、第１、第２の関節群動作制御部１８ｄ、１８ｆ）が実行する第１のモード実行処理について説明する。第１のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第１のモードを表している場合に、所定時間（例えば、１０［msec］）が経過するたびに実行される。
図３に示すように、まず、ステップＳ１０１では、第２の目標値演算部１８ｅは、指示入力部１５が出力する第４、第５の関節１２ｄ、１２ｅの動作の指示入力に基づき、第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/dt、dθ5ref/dtを算出する。
【0022】
続いてステップＳ１０２に移行して、第１の目標値演算部１８ａは、ステップＳ１０１で算出した目標値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については後述する。
〔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については後述する。
【0023】
続いてステップＳ１０３に移行して、第１の関節群動作制御部１８ｄは、ステップＳ１０２で算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値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を制御する。
【0024】
また同時に、第２の関節群動作制御部１８ｆは、ステップＳ１０１で算出した第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/dt、dθ5ref/dt、及び動作量検出部１３ｄ、１３ｅの検出結果に基づき、第４、第５の関節１２ｄ、１２ｅの角速度dθ4/dt、dθ5/dtを目標値dθ4ref/dt、dθ5ref/dtと等しくするための指令を関節駆動装置８に出力した後、この演算処理、つまり、第１のモード実行処理を終了する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第４、第５の関節１２ｄ、１２ｅを駆動させ、第４、第５の関節１２ｄ、１２ｅの動作速度dθ4/dt、dθ5/dtを制御する。
これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１～第５の関節１２ａ～１２ｅそれぞれを駆動させることで、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、穿孔ロッド７ａの姿勢を操作可能とする。
これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１～第５の関節１２ａ～１２ｅそれぞれを駆動させることで、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、穿孔ロッド７ａの姿勢を操作可能とする。
【0025】
ここで、穿孔装置４は、穿孔ロッド７ａの先端部の位置を決めるために３自由度、穿孔ロッド７ａの角度を決めるために２自由度、合計５自由度が必要となる。これに加え、操縦席からの視認性確保や、トンネル壁面との衝突回避、トンネル側壁面への穿孔を行うために、自由度に冗長性を持たせる必要があるため、合計８自由度を有している。そのため、例えば、穿孔ロッド１７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtから、その移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtをゼロとするための、８自由度に対応する関節、つまり第１～第８の関節１２ａ～１２ｈの動作速度の目標値を行列式で算出するようにした場合、入力と出力の数が異なるため、係数行列の逆行列を直接計算できない。したがって、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、穿孔ロッド７ａの姿勢を操作可能とすることが困難である。
【0026】
これに対し、実施形態に係る穿孔支援装置１によれば、第１～第３の関節１２ａ～１２ｃを駆動することで、他の関節の動作に起因して穿孔ロッド１７ａの先端部に発生することが予期される移動速度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は、３×３の正方行列となるため、直接計算できる。したがって、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、穿孔ロッド７ａの姿勢を操作可能とすることが容易となる。
【0027】
（第２のモード実行処理）
次に、コントローラ１８（第１、第２の目標値演算部１８ａ、１８ｅ、第１、第２の関節群動作制御部１８ｄ、１８ｆ）が実行する第２のモード実行処理について説明する。第２のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第２のモードを表している場合に、所定時間（１０［msec］）が経過するたびに実行される。
図５に示すように、まず、ステップＳ２０１では、第２の目標値演算部１８ｅは、指示入力部１５が出力する第６の関節１２ｆの動作（回転）の指示入力に基づき、第６の関節１２ｆの角速度の目標値dθ6ref/dtを算出する。
次に、コントローラ１８（第１、第２の目標値演算部１８ａ、１８ｅ、第１、第２の関節群動作制御部１８ｄ、１８ｆ）が実行する第２のモード実行処理について説明する。第２のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第２のモードを表している場合に、所定時間（１０［msec］）が経過するたびに実行される。
図５に示すように、まず、ステップＳ２０１では、第２の目標値演算部１８ｅは、指示入力部１５が出力する第６の関節１２ｆの動作（回転）の指示入力に基づき、第６の関節１２ｆの角速度の目標値dθ6ref/dtを算出する。
【0028】
続いてステップＳ２０２に移行して、第１の目標値演算部１８ａは、ステップＳ２０１で算出した目標値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はゼロとする。
【0029】
続いてステップＳ２０３に移行して、第１の関節群動作制御部１８ｄは、ステップＳ２０２で算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値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を制御する。
【0030】
また同時に、第２の関節群動作制御部１８ｆは、ステップＳ２０１で算出した第６の関節１２ｆの角速度の目標値dθ6ref/dt、及び動作量検出部１３ｆの検出結果に基づき、第６の関節１２ｆの角速度dθ6/dtを目標値dθ6ref/dtと等しくするための指令を関節駆動装置８に出力した後、この演算処理、つまり、第２のモード実行処理を終了する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第６の関節１２ｆを駆動させ、第６の関節１２ｆの動作速度dθ6/dtを制御する。
これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１～第３、第６の関節１２ａ～１２ｃ、１２ｆそれぞれを駆動させることで、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、第６の回転角θ6を操作可能とする。その際、第７の関節１２ｇにおける第７の回転角θ7が０［rad］である場合には、穿孔ロッド７ａの先端部の位置に加え、穿孔ロッド７ａの姿勢も維持される。
これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１～第３、第６の関節１２ａ～１２ｃ、１２ｆそれぞれを駆動させることで、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、第６の回転角θ6を操作可能とする。その際、第７の関節１２ｇにおける第７の回転角θ7が０［rad］である場合には、穿孔ロッド７ａの先端部の位置に加え、穿孔ロッド７ａの姿勢も維持される。
【0031】
（第３のモード実行処理）
次に、コントローラ１８（第１、第２の目標値演算部１８ａ、１８ｅ、第１、第２の関節群動作制御部１８ｄ、１８ｆ）が実行する第３のモード実行処理について説明する。第３のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第３のモードを表している場合に、所定時間（１０［msec］）が経過するたびに実行される。
図６に示すように、まず、ステップＳ３０１では、第２の目標値演算部１８ｅは、指示入力部１５が出力する第７の関節１２ｇの動作（回転）の指示入力に基づき、第７の関節１２ｇの角速度の目標値dθ7ref/dtを算出する。
次に、コントローラ１８（第１、第２の目標値演算部１８ａ、１８ｅ、第１、第２の関節群動作制御部１８ｄ、１８ｆ）が実行する第３のモード実行処理について説明する。第３のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第３のモードを表している場合に、所定時間（１０［msec］）が経過するたびに実行される。
図６に示すように、まず、ステップＳ３０１では、第２の目標値演算部１８ｅは、指示入力部１５が出力する第７の関節１２ｇの動作（回転）の指示入力に基づき、第７の関節１２ｇの角速度の目標値dθ7ref/dtを算出する。
【0032】
続いてステップＳ３０２に移行して、第１の目標値演算部１８ａは、ステップＳ３０１で算出した目標値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はゼロとする。
【0033】
続いてステップＳ３０３に移行して、第１の関節群動作制御部１８ｄは、ステップＳ３０２で算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値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を制御する。
【0034】
また同時に、第２の関節群動作制御部１８ｆは、ステップＳ３０１で算出した第７の関節１２ｇの角速度の目標値dθ7ref/dt、及び動作量検出部１３ｇの検出結果に基づき、第７の関節１２ｇの角速度dθ7/dtを目標値dθ7ref/dtと等しくするための指令を関節駆動装置８に出力した後、この演算処理、つまり、第３のモード実行処理を終了する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第７の関節１２ｇを駆動させ、第７の関節１２ｇの動作速度dθ7/dtを制御する。
これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１～第３、第７の関節１２ａ～１２ｃ、１２gそれぞれを駆動させることで、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、第７の回転角θ7を操作可能とする。なお、第７の回転角θ7を操作する状況としては、ロックボルト穿孔がある。
これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１～第３、第７の関節１２ａ～１２ｃ、１２gそれぞれを駆動させることで、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータからの指示入力に従って、第７の回転角θ7を操作可能とする。なお、第７の回転角θ7を操作する状況としては、ロックボルト穿孔がある。
【0035】
（第４のモード実行処理）
次に、コントローラ１８（第１の目標値演算部１８ａ、第１、第２の演算実行部１８ｂ、１８ｃ、第１の関節群動作制御部１８ｄ、第２の目標値演算部１８ｅ、第２の関節群動作制御部１８ｆ、位置取得部１８ｇ）が実行する第４のモード実行処理について説明する。第４のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第４のモードを表している場合に、所定時間（１０［msec］）が経過するたびに実行される。
図７に示すように、まず、ステップＳ４０１では、第２の目標値演算部１８ｅは、計画孔記憶部１６が記憶している切羽２の穿孔パターンから計画孔を１つ選択する。計画孔の選択方法としては、例えば、穿孔装置４のオペレータによる指示入力部１５の操作を基に選択する方法や、予め定めた順番で自動的に選択する方法を用いることができる。続いて、選択した計画孔の開口位置ｘref、ｙref、ｚref及び挿し角φref、ψrefを読み出す。
次に、コントローラ１８（第１の目標値演算部１８ａ、第１、第２の演算実行部１８ｂ、１８ｃ、第１の関節群動作制御部１８ｄ、第２の目標値演算部１８ｅ、第２の関節群動作制御部１８ｆ、位置取得部１８ｇ）が実行する第４のモード実行処理について説明する。第４のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第４のモードを表している場合に、所定時間（１０［msec］）が経過するたびに実行される。
図７に示すように、まず、ステップＳ４０１では、第２の目標値演算部１８ｅは、計画孔記憶部１６が記憶している切羽２の穿孔パターンから計画孔を１つ選択する。計画孔の選択方法としては、例えば、穿孔装置４のオペレータによる指示入力部１５の操作を基に選択する方法や、予め定めた順番で自動的に選択する方法を用いることができる。続いて、選択した計画孔の開口位置ｘref、ｙref、ｚref及び挿し角φref、ψrefを読み出す。
【0036】
続いてステップＳ４０２に移行して、第２の目標値演算部１８ｅは、Σ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
ここで、ｋψ、ｋφは、フィードバックゲインである。
φnow=tan-1(Ｒ12/Ｒ22)
ψnow=tan-1(Ｒ32/Ｒ22) …（２）
dθ4ref/dt=ｋψ・ψerr
dθ5ref/dt=ｋφ・φerr …（３）
ψerr=ψref-ψnow
φerr=φref-φnow
ここで、ｋψ、ｋφは、フィードバックゲインである。
【0037】
続いてステップＳ４０３に移行して、位置取得部１８ｇは、穿孔ロッド７ａの先端部の位置ｘnow、ｙnow、ｚnowを取得する。位置ｘnow、ｙnow、ｚnowを取得する方法としては、例えば、穿孔装置４とは別体に設けられ、穿孔ロッド７ａの先端部の位置を測量する測量装置（不図示）から取得する方法や、動作量検出部１３ａ～１３ｈの検出結果（回転量、伸縮量）に基づき、穿孔ロッド７ａの先端部の位置を算出する方法を採用できる。
続いてステップＳ４０４に移行して、第１の目標値演算部１８ａ（第１の演算実行部１８ｂ）は、ステップＳ４０１で読み出した計画孔の開口位置（ｘref、ｙref、ｚref）と、ステップＳ４０３で取得した穿孔ロッド７ａの先端部の位置（ｘnow、ｙnow、ｚnow）との差（距離）をゼロとするための、穿孔ロッド７ａの先端部の移動速度の目標値（以下、「目標速度Ｖref（=〔d0ｘ8ref/dt、d0ｙ8ref/dt、d0ｚ8ref/dt〕T）」とも呼ぶ）を算出する。
続いてステップＳ４０４に移行して、第１の目標値演算部１８ａ（第１の演算実行部１８ｂ）は、ステップＳ４０１で読み出した計画孔の開口位置（ｘref、ｙref、ｚref）と、ステップＳ４０３で取得した穿孔ロッド７ａの先端部の位置（ｘnow、ｙnow、ｚnow）との差（距離）をゼロとするための、穿孔ロッド７ａの先端部の移動速度の目標値（以下、「目標速度Ｖref（=〔d0ｘ8ref/dt、d0ｙ8ref/dt、d0ｚ8ref/dt〕T）」とも呼ぶ）を算出する。
【0038】
具体的には、第１の目標値演算部１８ａ（第１の演算実行部１８ｂ）は、まず下記（４）式に従って正規化した位置誤差ベクトルＱ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 …（５）
Ｑ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 …（５）
【0039】
ここで、Ｓ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の時間変化率を徐々に低減させる。これにより、穿孔ロッド７ａの先端部の移動開始時に移動速度をスローアップし、移動終了時に移動速度をスローダウンすることができるため、第１～第３の関節１２ａ～１２ｃの振動を抑制することができる。
【0040】
ちなみに、穿孔装置４では、第１～第３の関節１２ａ～１２ｃを、電動モータ等と異なり応答速度が遅い油圧駆動装置を用いて駆動している。それゆえ、例えば、目標値d0ｘ8ref/dt、d0ｙ8ref/dt、d0ｚ8ref/dtの時間変化率が増大し、第１～第３の関節１２ａ～１２ｃの動作速度の目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtが増大すると、応答遅れやオーバーシュートが発生して、第１～第３の関節１２ａ～１２ｃに振動が発生する可能性がある。
【0041】
続いてステップＳ４０５に移行して、第１の目標値演算部１８ａ（第２の演算実行部１８ｃ）は、ステップＳ４０２で算出した目標値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を算出する。
【0042】
具体的には、第１の目標値演算部１８ａ（第２の演算実行部１８ｃ）は、まず、目標値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） …（７）
〔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） …（７）
【0043】
続いてステップＳ４０６に移行して、第１の関節群動作制御部１８ｄは、ステップＳ４０５で算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値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を制御する。
【0044】
同時に、第２の関節群動作制御部１８ｆは、ステップＳ４０２で算出した第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/dt、dθ5ref/dt、及び動作量検出部１３ｄ、１３ｅの検出結果に基づき、第４、第５の関節１２ｄ、１２ｅの角速度dθ4/dt、dθ5/dtを目標値dθ4ref/dt、dθ5ref/dと等しくするための指令を関節駆動装置８に出力した後、この演算処理、つまり、第４のモード実行処理を終了する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第４、第５の関節１２ｄ、１２ｅを駆動させ、第４、第５の関節１２ｄ、１２ｅの動作速度dθ4/dt、dθ5/dtを制御する。
これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１～第５の関節１２ａ～１２ｅそれぞれを駆動させることで、穿孔ロッド７ａの先端部の位置と姿勢とを、計画孔の開口位置と挿し角とに自動的にゆっくりと徐々に近づけることができる。
これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第１～第５の関節１２ａ～１２ｅそれぞれを駆動させることで、穿孔ロッド７ａの先端部の位置と姿勢とを、計画孔の開口位置と挿し角とに自動的にゆっくりと徐々に近づけることができる。
【0045】
（第５のモード実行処理）
次に、コントローラ１８（第１、第２の目標値演算部１８ａ、１８ｅ、第１、第２の関節群動作制御部１８ｄ、１８ｆ）が実行する第５のモード実行処理について説明する。第５のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第５のモードを表している場合に、所定時間（１０［msec］）が経過するたびに実行される。
図９に示すように、まず、ステップＳ５０１では、第１の目標値演算部１８ａは、指示入力部１５が出力する第１～第３の関節１２ａ～１２ｃの動作の指示入力に基づき、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する。また、第２の目標値演算部１８ｅは、指示入力部１５が出力する第４～第７の関節１２ｄ～１２ｇの動作の指示入力に基づき、第４～第７の関節１２ｄ～１２ｇの角速度の目標値dθ4ref/dt～dθ7ref/dtを算出する。
次に、コントローラ１８（第１、第２の目標値演算部１８ａ、１８ｅ、第１、第２の関節群動作制御部１８ｄ、１８ｆ）が実行する第５のモード実行処理について説明する。第５のモード実行処理は、モード切替スイッチ１４が出力する指示入力が第５のモードを表している場合に、所定時間（１０［msec］）が経過するたびに実行される。
図９に示すように、まず、ステップＳ５０１では、第１の目標値演算部１８ａは、指示入力部１５が出力する第１～第３の関節１２ａ～１２ｃの動作の指示入力に基づき、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する。また、第２の目標値演算部１８ｅは、指示入力部１５が出力する第４～第７の関節１２ｄ～１２ｇの動作の指示入力に基づき、第４～第７の関節１２ｄ～１２ｇの角速度の目標値dθ4ref/dt～dθ7ref/dtを算出する。
【0046】
続いてステップＳ５０２に移行して、第１の関節群動作制御部１８ｄは、ステップＳ５０１で算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値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を制御する。
【0047】
また同時に、第２の関節群動作制御部１８ｆは、ステップＳ５０１で算出した第４～第７の関節１２ｄ～１２ｇの角速度の目標値dθ4ref/d～dθ7ref/dt、及び動作量検出部１３ｄ～１３ｇの検出結果に基づき、第４～第７の関節１２ｄ～１２ｇの角速度dθ4/dt～dθ7/dtを目標値dθ4ref/dt～dθ7ref/dtと等しくするための指令を関節駆動装置８に出力した後、この演算処理、つまり、第５のモード実行処理を終了する。これにより、関節駆動装置８に、コントローラ１８からの指令に従って、第４～第７の関節１２ｄ～１２ｇを駆動させ、第４～第７の関節１２ｄ～１２ｇの動作速度dθ4/dt～dθ7/dtを制御する。
【0048】
（演算式の導出手順）
次に、上記（１）式の導出手順について詳細に説明する。
まず、図１０（ａ）（ｂ）に示すように、Σ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座標系の原点を見た場合の位置を、下記表１に示す。
次に、上記（１）式の導出手順について詳細に説明する。
まず、図１０（ａ）（ｂ）に示すように、Σ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座標系の原点を見た場合の位置を、下記表１に示す。
【0049】
【表1】
【0050】
すると、Σ0座標系とΣ1座標系との同次変換行列0Ｔ1は、下記（８）式で表される。
【数1】
【0051】
ここで、C1=cosθ1、S1=sinθ1である。
また、Σ1座標系とΣ2座標系との同次変換行列1Ｔ2は、下記（９）式で表される。
また、Σ1座標系とΣ2座標系との同次変換行列1Ｔ2は、下記（９）式で表される。
【数2】
【0052】
ここで、C2=cosθ2、S2=sinθ2である。
また、Σ2座標系とΣ3座標系との同次変換行列2Ｔ3は、下記（１０）式で表される。
また、Σ2座標系とΣ3座標系との同次変換行列2Ｔ3は、下記（１０）式で表される。
【数3】
【0053】
また、Σ3座標系とΣ4座標系との同次変換行列3Ｔ4は、下記（１１）式で表される。
【数4】
【0054】
ここで、C4=cosθ4、S4=sinθ4である。
また、Σ4座標系とΣ5座標系との同次変換行列4Ｔ5は、下記（１２）式で表される。
また、Σ4座標系とΣ5座標系との同次変換行列4Ｔ5は、下記（１２）式で表される。
【数5】
【0055】
ここで、C5=cosθ5、S5=sinθ5である。
また、Σ5座標系とΣ6座標系との同次変換行列5Ｔ6は、下記（１３）式で表される。
また、Σ5座標系とΣ6座標系との同次変換行列5Ｔ6は、下記（１３）式で表される。
【数6】
【0056】
ここで、C6=cosθ6、S6=sinθ6である。
また、Σ6座標系とΣ7座標系との同次変換行列6Ｔ7は、下記（１４）式で表される。
また、Σ6座標系とΣ7座標系との同次変換行列6Ｔ7は、下記（１４）式で表される。
【数7】
【0057】
ここで、C7=cosθ7、S7=sinθ7である。
また、Σ7座標系とΣ8座標系との同次変換行列7Ｔ8は、下記（１５）式で表される。
また、Σ7座標系とΣ8座標系との同次変換行列7Ｔ8は、下記（１５）式で表される。
【数8】
【0058】
また、Σ0座標系とΣ8座標系との同次変換行列0Ｔ8は、下記（１６）式で表される。
【数9】
【0059】
以上より、Σ0座標系から見たΣ8座標系の原点の座標は下記（１７）式で表される。
0ｘ8=C1(C5(S6(Ｌ8S7+C7Hshell+Hdump)-Drote-C6Ddump)-S5((-HshellS7)+Lrote+C7Ｌ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
【数10】
-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
【0060】
ここで、各関節１２ａ～１２ｈの状態量がθ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を乗算することで得られる。
【0061】
すなわち、チルトボディ９ｅを第４の関節１２ｄ回りに角速度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
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
【0062】
また、スイングボディ９ｆを第５の関節１２ｅ回りに角速度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)
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)
【0063】
さらに、ロータリーボディ９ｇを第６の関節１２ｆ回りに角速度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
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
【0064】
また、ガイドマウンチング９ｉを第７の関節１２ｇ回りに角速度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
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
【0065】
また、第４～第７の関節１２ｄ～１２ｇの動作を同時に行った場合には、上記（１８）～（２９）式のＸ方向成分d0ｘ8/dt、Ｙ方向成分d0ｙ8/dt、Ｚ方向成分d0ｚ8/dtを成分毎に足し合わせた速度が、穿孔ロッド７ａの先端部に発生する。それゆえ、穿孔ロッド７ａの先端部に発生する移動速度のＸ方向成分d0ｘ8/dt、Ｙ方向成分d0ｙ8/dt、Ｚ方向成分d0ｚ8/dtは、下記（３０）式に示すように、行列式で表すことができる。
【0066】
【数11】
【0067】
ここで、第４～第７の関節１２ｄ～１２ｇの角速度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)を更新する必要はない。
【0068】
また、第１～第８の関節１２ａ～１２ｈの状態量がθ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
ここで、基点の座標の各成分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
【0069】
続いて、上記（３１）、（３２）式及び省略した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 …（３５）
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 …（３５）
【0070】
また、第１～第３の関節１２ａ～１２ｃの動作を同時に行った場合には、上記（３３）～（３５）式のＸ方向成分d0ｘ5/dt、Ｙ方向成分d0ｙ5/dt、Ｚ方向成分d0ｚ5/dtを成分毎に足し合わせた速度が、位置合わせの基点に発生する。それゆえ、基点に発生する移動速度のＸ方向成分d0ｘ5/dt、Ｙ方向成分d0ｙ5/dt、Ｚ方向成分d0ｚ5/dtは、上記（３０）式と同様に、下記（３６）式に示すように、行列式で表すことができる。
【0071】
【数12】
【0072】
ここで、第１～第３の関節１２ａ～１２ｃの角速度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）を演算可能な下記（３７）式が導出される。
【0073】
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)
【0074】
また、穿孔装置４は、第１の回転角θ1と第５の回転角θ5、第２の回転角θ2と第４の回転角θ4の同調機能を有しているため、下記（３８）式に示すように、位置合わせの基点の移動速度d0ｘ5/dt、d0ｙ5/dt、d0ｚ5/dtは、穿孔ロッド７ａの先端部の移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtと等しくなる。
【0075】
【数14】
【0076】
このような手順により、上記（３０）、（３７）、（３８）式、つまり、上記（１）式が導出される。
（動作その他）
次に、実施形態に係る穿孔支援装置１の動作について説明する。
まず、穿孔装置４のオペレータが、モード切替スイッチ１４を操作し、動作モードを第１のモードに切り替えたとする。そして、指示入力部１５を操作し、第４、第５の関節１２ｄ、１２ｅを動作させる指示入力、つまり、穿孔ロッド７ａの姿勢を変更させる入力を行ったとする。すると、コントローラ１８が、指示入力部１５が出力する第４、第５の関節１２ｄ、１２ｅの動作、つまり、回転の指示入力に基づき、第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/dt、dθ5ref/dtを算出する（図３のステップＳ１０１）。
（動作その他）
次に、実施形態に係る穿孔支援装置１の動作について説明する。
まず、穿孔装置４のオペレータが、モード切替スイッチ１４を操作し、動作モードを第１のモードに切り替えたとする。そして、指示入力部１５を操作し、第４、第５の関節１２ｄ、１２ｅを動作させる指示入力、つまり、穿孔ロッド７ａの姿勢を変更させる入力を行ったとする。すると、コントローラ１８が、指示入力部１５が出力する第４、第５の関節１２ｄ、１２ｅの動作、つまり、回転の指示入力に基づき、第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/dt、dθ5ref/dtを算出する（図３のステップＳ１０１）。
【0077】
続いて、コントローラ１８が、算出した目標値dθ4ref/dt、dθ5ref/dtに基づき、上記（１）式に従って、第４、第５の関節１２ｄ、１２ｅの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtをゼロとするための、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する（図３のステップＳ１０２）。
【0078】
続いて、コントローラ１８が、算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値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が打ち消され、穿孔ロッド７ａの先端部の位置が維持される。
【0079】
同時に、コントローラ１８が、算出した第４、第５の関節１２ｄ、１２ｅの角速度の目標値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と等しくなる。それゆえ、オペレータの操作に応じて、穿孔ロッド７ａの姿勢が変更される。
そして、上記フローが繰り返し実行されることで、穿孔ロッド７ａの先端部の位置を維持しつつ、穿孔装置４のオペレータが希望する穿孔ロッド７ａの姿勢に変更することができる。そのため、穿孔ロッド７ａの位置及び姿勢をより容易に調整することができる。
そして、上記フローが繰り返し実行されることで、穿孔ロッド７ａの先端部の位置を維持しつつ、穿孔装置４のオペレータが希望する穿孔ロッド７ａの姿勢に変更することができる。そのため、穿孔ロッド７ａの位置及び姿勢をより容易に調整することができる。
【0080】
ここで、トンネル工事では、穿孔装置４を使用して切羽２に複数の装薬孔３を穿孔しながら掘り進めてゆくが、経済性や効率性を考慮すると、できるだけ余掘りを少なくすることが重要である。なぜならば、余掘りが多くなると、余分な量のずりと呼ばれる掘削残渣が発生するだけでなく、仕上げコンクリートの施工量も余分に必要となるからである。
したがって、経済性や効率性の高いトンネル工事のためには、予定した切羽２の位置へ計画した深さの装薬孔３を掘ることが非常に重要である。そのため、オペレータは、穿孔ロッド７ａの先端を、切羽２の計画した位置に正確に設置する必要がある。このとき、オペレータは、目視によって穿孔ロッド７ａの先端が正確に目的の箇所を指しているかを確認する必要があるが、穿孔機７の姿勢によっては目視ができない場合がある。このような場合には、穿孔装置４の各関節１２ａ～１２ｈを動作させて目視可能な位置まで穿孔機７を移動させることになるが、操作の良し悪しによっては、穿孔ロッド７ａの先端位置がずれてしまったり、挿し角が変わってしまったりということが起こり得る。また、これらの操作に余分な時間を費やすこととなり、作業工程が長くなってしまうこととなる。
したがって、経済性や効率性の高いトンネル工事のためには、予定した切羽２の位置へ計画した深さの装薬孔３を掘ることが非常に重要である。そのため、オペレータは、穿孔ロッド７ａの先端を、切羽２の計画した位置に正確に設置する必要がある。このとき、オペレータは、目視によって穿孔ロッド７ａの先端が正確に目的の箇所を指しているかを確認する必要があるが、穿孔機７の姿勢によっては目視ができない場合がある。このような場合には、穿孔装置４の各関節１２ａ～１２ｈを動作させて目視可能な位置まで穿孔機７を移動させることになるが、操作の良し悪しによっては、穿孔ロッド７ａの先端位置がずれてしまったり、挿し角が変わってしまったりということが起こり得る。また、これらの操作に余分な時間を費やすこととなり、作業工程が長くなってしまうこととなる。
【0081】
これに対し、実施形態に係る穿孔支援装置１によれば、穿孔ロッド７ａの位置及び姿勢を容易に調整できるため、穿孔装置４の各関節１２ａ～１２ｈを動作させて目視可能な位置まで穿孔機７を容易に移動できる。それゆえ、穿孔ロッド７ａの先端を、切羽２の計画した位置に正確に設置することができ、切羽２への正確な発破孔施工によって余掘りを少なくし、経済性や効率性高くトンネル工事を行うことができる。また、穿孔ロッド７ａの位置及び姿勢の調整に費やす時間を短縮でき、作業工程を短縮できる。また、オペレータの熟達した技術が要求されず、しかも高価なセンサー類や測定装置が必要とされない。
【0082】
一方、穿孔装置４のオペレータが、モード切替スイッチ１４を操作し、動作モードを第２のモードに切り替えたとする。そして、指示入力部１５を操作し、第６の関節１２ｆを動作させる指示入力、つまり、穿孔ロッド７ａの第６の回転角θ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を算出する（図５のステップＳ２０２）。
続いて、コントローラ１８が、算出した目標値dθ6ref/dtに基づき、上記（１）式に従って、第６の関節１２ｆの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtをゼロとするための、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する（図５のステップＳ２０２）。
【0083】
続いて、コントローラ１８が、算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値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が打ち消され、穿孔ロッド７ａの先端部の位置が維持される。
【0084】
同時に、算出した第６の関節１２ｆの角速度の目標値dθ6ref/dt、動作量検出部１３ｆの検出結果に基づき、第６の関節１２ｆの角速度dθ6/dtを目標値dθ6ref/dtと等しくするための指令を関節駆動装置８に出力する（図５のステップＳ２０３）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第６の関節１２ｆを駆動させ、動作速度dθ6/dtが目標値dθ6ref/dtと等しくなり、第６の回転角θ6が変更される。
そして、上記フローが繰り返し実行されることで、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータが希望する第６の回転角θ6に変更できる。その際、第７の関節１２ｇの回転角度θ7が０［rad］である場合には、穿孔ロッド７ａの姿勢も維持できる。
そして、上記フローが繰り返し実行されることで、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータが希望する第６の回転角θ6に変更できる。その際、第７の関節１２ｇの回転角度θ7が０［rad］である場合には、穿孔ロッド７ａの姿勢も維持できる。
【0085】
また一方、穿孔装置４のオペレータが、モード切替スイッチ１４を操作し、動作モードを第３のモードに切り替えたとする。そして、指示入力部１５を操作し、第７の関節１２ｇを動作させる指示入力、つまり、穿孔ロッド７ａの第７の回転角θ7（ガイドダンプ角）を変更させる入力を行ったとする。すると、コントローラ１８が、指示入力部１５が出力する第７の関節１２ｇの動作の指示入力、つまり、回転の指示入力に基づき、第７の関節１２ｇの角速度の目標値dθ7ref/dtを算出する（図５のステップＳ３０１）。
続いて、コントローラ１８が、取得した第７の回転角θ7に基づき、上記（１）式に従って、第７の関節１２ｇの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtをゼロとするための、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する（図６のステップＳ３０２）。
続いて、コントローラ１８が、取得した第７の回転角θ7に基づき、上記（１）式に従って、第７の関節１２ｇの動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度d0ｘ8/dt、d0ｙ8/dt、d0ｚ8/dtをゼロとするための、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する（図６のステップＳ３０２）。
【0086】
続いて、コントローラ１８が、算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値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が打ち消され、穿孔ロッド７ａの先端部の位置が維持される。
【0087】
同時に、算出した第７の関節１２ｇの角速度の目標値dθ7ref/dt、動作量検出部１３ｇの検出結果に基づき、第７の関節１２ｇの動作速度dθ7/dtを目標値dθ7ref/dtと等しくするための指令を関節駆動装置８に出力する（図６のステップＳ３０３）。これにより、関節駆動装置８が、コントローラ１８からの指令に従って、第７の関節１２ｇを駆動させ、動作速度dθ7/dtが目標値dθ7ref/dtと等しくなり、第７の回転角θ7が変更される。
そして、上記フローが繰り返し実行されることで、穿孔ロッド７ａの先端部の位置を自動的に維持しつつ、穿孔装置４のオペレータが希望する第７の回転角θ7に変更できる。
そして、上記フローが繰り返し実行されることで、穿孔ロッド７ａの先端部の位置を自動的に維持しつつ、穿孔装置４のオペレータが希望する第７の回転角θ7に変更できる。
【0088】
また一方、穿孔装置４のオペレータが、モード切替スイッチ１４を操作し、動作モードを第４のモードに切り替えたとする。すると、コントローラ１８が、計画孔記憶部１６が記憶している切羽２の穿孔パターンから計画孔を１つ選択し、選択した計画孔の開口位置ｘref、ｙref、ｚref及び挿し角φref、ψrefを読み出す（図７のステップＳ４０１）。続いて、挿し角φnow、ψnowを算出し、算出した挿し角φnow、ψnow、及び読み出した計画孔の挿し角φref、ψrefに基づき、上記（３）式に従って第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/dt、dθ5ref/dtを算出する（図７のステップＳ４０２）。
【0089】
続いて、コントローラ１８が、穿孔ロッド７ａの先端部の位置ｘ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を算出する（図７のステップＳ４０５）。
続いて、コントローラ１８が、算出した目標速度Ｖ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を算出する（図７のステップＳ４０５）。
【0090】
続いて、コントローラ１８が、算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値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が打ち消される。
【0091】
また同時に、算出した第４、第５の関節１２ｄ、１２ｅの角速度の目標値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と等しくなる。
そして、上記フローが繰り返し実行されることで、穿孔ロッド７ａの先端部の位置と姿勢とを、計画孔の開口位置と挿し角とに自動的に速やかに近づけることができる。
そして、上記フローが繰り返し実行されることで、穿孔ロッド７ａの先端部の位置と姿勢とを、計画孔の開口位置と挿し角とに自動的に速やかに近づけることができる。
【0092】
また一方、穿孔装置４のオペレータが、モード切替スイッチ１４を操作して、動作モードを第５のモードに切り替えたとする。そして、指示入力部１５を操作し、第４、第５の関節１２ｄ、１２ｅを動作させる指示入力、つまり、穿孔ロッド７ａの姿勢を変更させる入力を行ったとする。すると、コントローラ１８が、指示入力部１５が出力する第４、第５の関節１２ｄ、１２ｅの動作（回転）の指示入力に基づき、第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/dt、dθ5ref/dtを算出する（図９のステップＳ５０１）。
【0093】
続いて、コントローラ１８が、算出した第４、第５の関節１２ｄ、１２ｅの角速度の目標値dθ4ref/d、dθ5ref/dt、及び動作量検出部１３ｄ、１３ｅの検出結果に基づき、第４、第５の関節１２ｄ、１２ｅの角速度dθ4/dt、dθ5/dtを目標値dθ4ref/dt、dθ5ref/dtと等しくするための指令を関節駆動装置８に出力する（図９のステップＳ５０２）。そして、上記フローが繰り返し実行され、穿孔装置４のオペレータからの指示入力に応じて、第４、第５の回転角θ4、θ5、つまり、穿孔ロッド７ａの姿勢（挿し角）を変更できる。
【0094】
穿孔ロッド７ａの姿勢（挿し角）が定まった後、穿孔装置４のオペレータが、指示入力部１５を操作し、第１～第３の関節１２ａ～１２ｃを動作させる指示入力、つまり、穿孔ロッド７ａの先端部の位置を変更させる入力を行ったとする。すると、コントローラ１８が、指示入力部１５が出力する第１～第３の関節１２ａ～１２ｃの動作の指示入力に基づき、第１、第２の関節１２ａ、１２ｂの角速度の目標値dθ1ref/dt、dθ2ref/dt、及び第３の関節１２ｃの伸縮速度の目標値dＬ3ref/dtを算出する（図９のステップＳ５０１）。
【0095】
続いて、コントローラ１８が、算出した第１、第２の関節１２ａ、１２ｂの角速度の目標値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の同調機能を有しているため、定めた挿し角が維持される。
【0096】
以上、本実施形態では、図２のペデステル９ａが第１の構成部品を構成する。以下同様に、図２のアウターブーム９ｂが第２の構成部品を構成する。さらに、図２のインナーブーム９ｃが第３の構成部品を構成する。また、図２の第１～第３の関節１２ａ～１２ｃが第１の関節群を構成する。さらに、第１、第２の関節１２ａ、１２ｂの角速度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がヤコビ逆行列を構成する。
【0097】
（１）このように、実施形態に係る穿孔支援装置１によれば、第１～第３の関節１２ａ～１２ｃ（以下、「第１の関節群」とも呼ぶ）以外の１または複数の関節（以下、「第２の関節群」とも呼ぶ）の動作速度の目標値に基づき、第２の関節群の動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度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を制御するようにした。それゆえ、第２の関節群の動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度が打ち消されるように、第１の関節群を動作できる。そのため、穿孔ロッド７ａの先端部の位置及び姿勢をより容易に調整可能な穿孔支援装置を１を提供できる。
【0098】
（２）また、実施形態に係る穿孔支援装置１によれば、指示入力部１５で受け付けた指示入力に基づき、第２の関節群の動作速度の目標値を算出するため、穿孔ロッド７ａの先端部の位置を維持しつつ、オペレータが希望する穿孔ロッド７ａの姿勢に変更できる。
【0099】
（３）また、実施形態に係る穿孔支援装置１によれば、第２の関節群の動作速度の目標値のベクトルに、その目標値のベクトルから穿孔ロッド７ａの先端部の移動速度のベクトルを算出するヤコビ行列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の算出を容易に行うことができる。
【0100】
（４）さらに、実施形態に係る穿孔支援装置１によれば、第２の関節群の動作速度の目標値、計画孔の開口位置、及び穿孔ロッド７ａの先端部の位置に基づき、第２の関節群の動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度をゼロとするとともに、計画孔の開口位置と穿孔ロッド７ａの先端部の位置とを等しくするための、第１の関節群の動作速度の目標値を算出するようにした。それゆえ、穿孔ロッド７ａの先端部の位置と姿勢とを、計画孔の開口位置と挿し角とに自動的に近づけることができる。
【0101】
（５）また、実施形態に係る穿孔支援装置１によれば、第２の関節群の動作に起因して穿孔ロッド７ａの先端部に発生することが予期される移動速度をゼロとするとともに、計画孔の開口位置と穿孔ロッド７ａの先端部の位置とを等しくするための、第１の関節群の動作速度の目標値dθ1ref/dt、dθ2ref/dt、dＬ3ref/dtを算出するようにした。それゆえ、第１の関節群の動作速度dθ1/dt、dθ2/dt、dＬ3/dtの制御を容易に行うことができる。
【0102】
（６）さらに、実施形態に係る穿孔支援装置１によれば、加速域、定速域及び減速域からなる速度パターンを設定するようにした。それゆえ、穿孔ロッド７ａの先端部の移動開始時に移動速度をスローアップし、移動終了時に移動速度をスローダウンすることができるため、第１～第３の関節１２ａ～１２ｃに発生する振動を抑制することができる。
【符号の説明】
【0103】
１…穿孔支援装置、２…切羽、３…装薬孔、４…穿孔装置、５…移動台車、５ａ…ヨーク、６…支持部材、７…穿孔機、７ａ…穿孔ロッド、７ｂ…ドリフタ、８…関節駆動装置、９…ブーム、９ａ…ペデステル、９ｂ…アウターブーム、９ｃ…インナーブーム、９ｄ…フォーク、９ｅ…チルトボディ、９ｆ…スイングボディ、９ｇ…ロータリーボディ、９ｈ…腕部、９ｉ…ガイドマウンチング、１０…ガイドシェル、１１ａ…第１の回転軸、１１ｂ…第２の回転軸、１１ｃ…第３の直動軸、１１ｄ…第４の回転軸、１１ｅ…第５の回転軸、１１ｆ…第６の回転軸、１１ｇ…第７の回転軸、１１ｈ…第８の直動軸、１２ａ…第１の関節、１２ｂ…第２の関節、１２ｃ…第３の関節、１２ｄ…第４の関節、１２ｅ…第５の関節、１２ｆ…第６の関節、１２ｇ…第７の関節、１２ｈ…第８の関節、１３ａ～１３ｈ…動作量検出部、１４…モード切替スイッチ、１５…指示入力部、１６…計画孔記憶部、１７…モニター、１８…コントローラ、１８ａ…第１の目標値演算部、１８ｂ…第１の演算実行部、１８ｃ…第２の演算実行部、１８ｄ…第１の関節群動作制御部、１８ｅ…第２の目標値演算部、１８ｆ…第２の関節群動作制御部、１８ｇ…位置取得部
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】