JP4944840B2 - 誘導システム及び誘導方法 - Google Patents

Description

本発明は、坑道やトンネルなどの閉鎖空間内を走行する無人搬送車などの移動体を壁面に沿って誘導する誘導システム及び誘導方法に関する。

建設工事中のトンネルなど、走行路面が汚れていたり、石などの異物が落ちていたりする場合には、特許文献１に記載の壁倣い方式の接触式ガイドが確実な方法として公知である。また、特許文献２に記載されているように、鉛直な側壁を設け、この側壁に光切断線を投影し、側壁からの反射光をカメラで撮像して側壁と搬送車との距離を求めて走行制御を行うようなガイド式の誘導方法も知られている。

特開平８−２５８７０５号公報 特開平８−８３１２４号公報

しかしながら、従来の誘導システムや誘導方法では、どうしても誘導のための付帯的な設備が必要であり、誘導設備の設置や撤去に伴う手間が増大して作業負担も大きくなる。特に、従来の誘導システムや誘導方法をトンネル工事などに適用した場合には、トンネル工事の進捗に伴って搬送経路が変化する度に、誘導用設備の増設や変更が必要となってしまうため、設備負担も大きくなってしまう。

本発明は、以上の課題を解決することを目的としており、移動体の誘導に要する設備負担を抑え、閉鎖空間内での移動体の安定した誘導を実現できる誘導システム及び誘導方法を提供することを目的とする。

本発明は、縦長の閉鎖空間内で、前記閉鎖空間の軸線に沿って柱状の閉鎖空間内で、閉鎖空間を形成する壁面に沿って移動体を誘導する誘導システムにおいて、移動体の走行方向の前方及び後方の少なくとも一方を向いて移動体に搭載されると共に、閉鎖空間を横断する断面形状に関する二次元情報を取得する走査手段と、閉鎖空間の軸線に沿った走行ルート上の移動体を基準にして走査手段で取得され得る基準断面形状を記憶する断面形状記憶手段と、走査手段で取得された断面形状と断面形状記憶手段に記憶された基準断面形状との幾何学的関係から、閉鎖空間の横断面上での移動体の位置及び姿勢の推定値を求め、推定値に基づいて移動体の走行制御を行う走行制御手段と、を備えることを特徴とする。

この誘導システムでは、閉鎖空間内に付帯設備を設けるのではなく閉鎖空間の軸線に沿った走行ルート上の移動体を基準にして走査手段で取得され得る基準断面形状を断面形状記憶手段で記憶している。さらに誘導システムは、走査手段で取得された断面形状と基準断面形状との幾何学的関係から、移動体の姿勢と位置との推定値を求め、その推定値に基づいて移動体の走行制御を行うことで走行ルートに沿った安定した誘導を実現している。従って、誘導用設備の設置に要する設備負担や作業負担を抑えることができ、特に、閉鎖空間内で走行ルートが変更になった場合であっても、プログラムで走行位置を変更することで容易に対応できる。その結果として、移動体の誘導に要する設備負担を抑え、閉鎖空間内での移動体の安定した誘導を実現できる。

さらに、走査手段は、移動体に一または複数搭載され、断面形状記憶手段は、走査手段に対応付けて複数の基準断面形状を記憶し、走行制御手段は、走査手段で取得された断面形状と、断面形状記憶手段に対応付けられて記憶されている複数の基準断面形状との幾何学的関係から、閉鎖空間の横断面上での移動体の位置及び姿勢の推定値を求め、推定値に基づいて前記移動体の走行制御を行うと好適である。複数の基準断面形状を記憶しているので、各基準断面形状から推定した位置と姿勢との推定結果の誤差を評価することで、閉鎖空間が一律に同一ではなく、場所に応じて異なるような複数の基準断面形状を有するような場合にも対応できる。

走査手段は、レーザースキャナであると好適である。レーザースキャナであれば、閉鎖空間の内径（幅）、すなわち走行ルートから壁面までの距離に関係なく観測誤差は一定である。従って、閉鎖空間の広狭の影響を受け難いため、特に壁面から離れた状態で移動体を誘導する必要がある場合に有効である。

また、本発明は、縦長の閉鎖空間内で、閉鎖空間の軸線に沿って移動体を誘導する誘導方法において、移動体の進行方向の前方及び後方の少なくとも一方における閉鎖空間を横断する断面形状に関する二次元情報を取得する走査ステップと、閉鎖空間の軸線に沿った走行ルート上の移動体を基準にして走査手段で取得され得る基準断面形状と、走査ステップで取得された断面形状とを対比し、その断面形状と基準断面形状との幾何学的関係から、閉鎖空間の横断面上での移動体の位置及び姿勢の推定値を求め、その推定値に基づいて移動体の走行制御を行う走行制御ステップと、を備えることを特徴とする。この発明によれば、移動体の誘導に要する設備負担を抑え、閉鎖空間内での移動体の安定した誘導を実現できる。

本発明によれば、移動体の誘導に要する設備負担を抑え、閉鎖空間内での移動体の安定した誘導を実現できる。

以下、図面を参照して本発明に係る誘導システムの好適な実施の形態について説明する。

図１に示されるように、トンネル施工に伴うセグメントＳｅなどの搬送や検査、その他の作業を迅速に行うために、坑道Ｔ内に規定された所定の走行ルートＲに沿って搬送車両３Ａを自動運転させる必要がある。本実施形態に係る誘導システム１Ａは、坑道Ｔ内の内壁（壁面）Ｔａに沿った相対誘導によって搬送車両３Ａを誘導し、搬送車両３Ａの自律運転を実現するシステムである。搬送車両３Ａは、移動体に相当する。

楕円柱状の閉鎖空間Ｓが形成された坑道Ｔ内には搬送車両３Ａの走行ルートＲが規定されている。図２または図３に示されるように、搬送車両３Ａは、転動する車輪５ａが設けられた車体部５と、車体部５の上部に設けられた荷台７と、車体部５の前後に設けられた障害物センサ・バンパースイッチ９と、操向車輪５ａの舵取り、車輪５ａの駆動及び停止を行う駆動装置１１と、バッテリ（図示せず）とを備えている。荷台７には、セグメントＳｅなどの積荷が積載され、駆動装置１１によって車輪５ａの回転や操舵角の変更が実行される。

また、搬送車両３Ａには、進行方向Ｄｍの前部に取り付けられたレーザースキャナ（走査手段）１３と、レーザースキャナ１３から入力されたデータに基づいて搬送車両３Ａの走行を制御する制御装置１５とが搭載されている。

レーザースキャナ１３は、搬送車両３Ａの進行方向Ｄｍの前方側に向けて円軌道を描くようにレーザービームを照射し、坑道Ｔの内壁Ｔａに反射して戻ってきたレーザービーム（以下、「反射光」という）を受信するセンサ部を有する。レーザースキャナ１３は、レーザービームを照射してから反射光を受信するまでの往復時間から測位対象物までの距離を計測し、さらに、その距離とレーザービームの照射方向とから測位対象物の座標データを取得して制御装置１５に入力する。この座標データは、閉鎖空間Ｓを横断する断面形状としての二次元データ（二次元情報）である。以下、この二次元データを観測断面データという。

制御装置１５は、ＣＰＵ、ＲＡＭ及びＲＯＭなどが実装された制御基板、入出力装置及び外部記憶装置などを備えている。制御装置１５は、ＣＰＵやＲＡＭなどのハードウェア上に所定のソフトウェアを読み込ませることにより、ＣＰＵの制御のもとで入出力装置などが動作して、所定の機能が実現される。制御装置１５で実行される機能について説明する。

制御装置１５は、断面形状記憶部（断面形状記憶手段）１５ａ及び走行制御部（走行制御手段）１５ｂとして機能する。断面形状記憶部１５ａは、閉鎖空間である坑道Ｔの横断面形状に関する二次元データ（以下、「基準断面データ」という）を記憶している。具体的には、坑道Ｔは略楕円柱状であるため、坑道Ｔの軸線（トンネル軸Ｌ）に沿った横断面形状に変化は少ないと仮定でき、断面形状記憶部１５ａには、坑道Ｔ内の代表的な基準断面データが記憶されている。なお、走行ルートＲを複数の区間に分割し、所定の区間ごとに基準断面データを複数記憶するようにしてもよい。

走行制御部１５ｂは、レーザースキャナ１３で取得された観測断面データから坑道Ｔ（閉鎖空間Ｓ）を横断する断面形状としての観測楕円Ｅｃ（図６参照）を推定する。さらに、断面形状記憶部１５ａに記憶されている基準断面データから基準楕円Ｅ_０を取得し、観測楕円Ｅｃと基準楕円Ｅ_０とを比較する。ここで、走行制御部１５ｂは、観測楕円Ｅｃと基準楕円Ｅ_０との幾何学的関係から、坑道Ｔの横断面上、すなわちトンネル軸Ｌに直交する断面上での搬送車両３Ａの姿勢角（姿勢）及び相対位置（位置）を推定値として求める。さらに、走行制御部１５ｂは、求めた推定値から走行ルートＲ上に搬送車両３Ａを戻すように駆動装置１１を制御して搬送車両３Ａの相対誘導を実現する。

なお、基準断面データを複数記憶している場合には、複数の基準楕円Ｅ_０を取得し、観測楕円Ｅｃと各基準楕円Ｅ_０とを比較してそれぞれ搬送車両３Ａの姿勢角（姿勢）及び相対位置（位置）を推定値として求める。そして、求めた姿勢角（姿勢）及び相対位置（位置）から想定されるレーザースキャナのデータと実際にレーザースキャナ１３から得られたデータとの誤差を評価して適切な姿勢角（姿勢）及び相対位置（位置）を求める。この場合には、閉鎖空間Ｓが一律に同一ではなく、場所に応じて異なるような複数の基準断面形状を有するような場合にも対応できる。

本実施形態では、搬送車両３Ａ、搬送車両３Ａに搭載されたレーザースキャナ１３、制御装置１５及び駆動装置１１によって誘導システム１Ａが構成される。

次に、誘導システム１Ａによる搬送車両３Ａの誘導方法について図面を参照しながら説明する。図４は、搬送車両３Ａの相対誘導の動作手順を説明するフローチャートである。

制御装置１５の走行制御部１５ｂは、搬送車両３Ａの初期位置を設定した後に、駆動装置１１を駆動させて走行を開始する。搬送車両３Ａが走行を開始すると、レーザースキャナ１３はレーザー走査を開始する。レーザースキャナ１３では、坑道Ｔ（閉鎖空間Ｓ）を横断する断面形状に関する計測データ（観測断面データ）を取得し（ステップＳ１）、制御装置１５の走行制御部１５ｂに入力する。走行制御部１５ｂは、観測断面データを受け付けると観測楕円Ｅｃを推定するための処理を実行する（ステップＳ２）。観測楕円Ｅｃの推定について具体的に説明する。

実際の環境では、レーザースキャナ１３の計測データには照明や路面など、坑道Ｔの内壁Ｔａ以外の計測結果が混在する。坑道Ｔの横断面が楕円か円の場合には、その計測点の軌跡は楕円（円も含む）となることがわかっているので、全計測点から楕円を見つければよい。大きな誤差データを含むデータから楕円を推定する方法は様々であるが、例えば以下の方法で楕円を推定することができる。

一般の楕円は平面上に５点が与えられれば一意に決まるが、実際には計測点には微小な測定誤差が含まれる。このため、この微小な誤差を平均化して打ち消すためには、５点より多い点を選び、最小二乗法などで観測楕円Ｅｃのパラメータを推定する。

観測楕円の推定が終了すると、走行制御部１５ｂは、断面形状記憶部１５ａに記憶されている基準断面データを読み出して基準楕円Ｅ_０を求め（ステップＳ３）、観測楕円Ｅｃと基準楕円Ｅ_０との幾何学的関係から姿勢角及び相対位置の計算処理を実行する（ステップＳ４）。観測楕円Ｅｃと基準楕円Ｅ_０との幾何学的関係から姿勢角及び相対位置を求める計算処理方法は様々であり、以下、その一例を説明する。

まず、坑道Ｔによって形成される略楕円柱状の閉鎖空間Ｓと、その閉鎖空間Ｓの横断面との幾何学的関係について、図６〜図８を参照して説明する。

図６には、観測楕円Ｅｃと基準楕円Ｅ_０とが示されている。図６では、トンネル軸Ｌ方向をＹ軸、トンネル軸Ｌを左右方向に直交する方向をＸ軸、トンネル軸Ｌを上下方向に直交する方向をＺ軸として示している。基準楕円Ｅ_０は、トンネル軸Ｌに対して垂直に切断した場合の横断面形状であり、図６では、基準楕円Ｅ_０の基準軸のうち、短軸はＸ軸上に規定され、長軸はＺ軸上に規定されている。また、観測楕円Ｅｃは、基準楕円Ｅ_０に交線ＡＢで交差する楕円として示しており、交点Ａ，Ｂは断面の中心Ｏに対して点対称である。ここで、Ｘ軸と交線ＡＢとの間の挟角を切断角φｃと定義する。観測楕円Ｅｃは、基準楕円Ｅ_０を、交線ＡＢを回転軸にφｒだけ回転させた平面に対して、基準楕円Ｅ_０をトンネル軸Ｌ方向に投影したものと考えられる。従って、観測楕円Ｅｃは、切断角φｃと回転角φｒの２つの角度で定義することができる。

図７及び図８は、基準楕円Ｅ_０と観測楕円Ｅｃとの平面幾何学的な関係を説明するための図であり、図７は、交線ＡＢを回転軸にして基準楕円Ｅ_０を回転角φｒだけ回転させ、観測楕円Ｅｃに基準楕円Ｅ_０を重ねた図である。観測楕円Ｅｃは、図７に示されるように、基準楕円Ｅ_０上の任意の点と交線ＡＢとの距離（垂線方向（ＣＤ）の距離）Ｌに一定の係数“Ｋ”を掛ける写像により、基準楕円Ｅ_０上の任意の点が移された点と考えることもできる。また、図８は、図７に示す基準楕円Ｅ_０と観測楕円Ｅｃとの関係を観測楕円Ｅｃの基準軸（長軸と短軸）を示すように書き直した図である。図８に示されるように、観測楕円Ｅｃの基準軸のうち、例えば短軸Ｆ_１Ｆ_２と交線ＡＢとの角度φｅ（図６及び図８参照）は一意に決まる。なお、図８中の線分Ｆ_３Ｆ_４は、観測楕円Ｅｃの基準軸のうちの長軸を示している。

以上の内容をまとめると、楕円柱状の閉鎖空間Ｓを横断する任意の平面で切り取った断面形状は楕円となることがわかる。さらに、その横断面である観測楕円Ｅｃは、切断角φｃと観測楕円Ｅｃの回転角φｒで決まる。このとき、観測楕円Ｅｃの基準軸（例えば、短軸）Ｆ_１Ｆ_２と観測楕円Ｅｃの回転軸である交線ＡＢとの角度φｅは一意に決まる。なお、以下の説明において、角度φｅは、交線ＡＢの傾き角φｅと称する。

なお、補足として、切断角φｃと回転角φｒとから観測楕円Ｅｃは一つに決まるが、厳密に言うと観測楕円Ｅｃの向きは表裏の２通りが存在する。また、傾き角φｅについても１８０度回転した２通りが存在する。つまり、切断角φｃと回転角φｒから決まる姿勢角は、表裏上下が反対となる４通りある。しかし、閉鎖空間Ｓ内の搬送車両３Ａを想定した場合、搬送車両３Ａの初期の進行方向と姿勢がわかっていれば、姿勢角の連続性などから、実用上一意に決まると考えられる。

次に、図５及び図９〜図１３を参照しながら、姿勢角及び相対位置の計算処理について説明する。図５は、姿勢角及び相対位置の計算処理の手順を示すフローチャートである。

まず、図９を参照して搬送車両３Ａの姿勢角、すなわちレーザースキャナ１３の姿勢角について説明する。図９は、レーザースキャナ１３の姿勢角を模式的に示す図である。レーザースキャナ１３の姿勢角は、レーザービームを走査する際の回転軸の方向であり、ピッチ角θｐ、ヨー角θｙ及びロール角θｒによって規定される。つまり、姿勢角を図９のＹ軸に合わせると、走査面（観測楕円）は、Ｘ−Ｚ平面、つまり基準楕円と一致する。図９では、レーザースキャナ１３の位置を原点Oに示しており、レーザースキャナ１３の向きはOMａのベクトルで示している。トンネル軸Ｌ方向はY軸方向である。また、点ＭａからＸ―Ｙ平面に垂直に降ろした点Ｍｂ及び点Ｍａを含み、且つＸ−Ｚ平面に平行な平面とＹ軸との交点を点Ｍｃとする。この場合、ピッチ角θｐは直線ＯＭａと直線ＯＭｂとの挟角であり、ヨー角θｙは直線ＯＭｃと直線ＯＭｂとの挟角である。ピッチ角θｐとヨー角θｙはオイラー角である。また、ロール角θｒは、直線ＯＭａを回転軸として、点Ｍａを通りＸ−Ｙ平面に平行となる線分ＭａＭｄを基準にした回転角である。なお、レーザースキャナ１３は、走行ルートＲ上、すなわち、閉鎖空間Ｓの左右方向の中心にあると仮定している。

（切断角計算）
切断角φｃは、図７に示される写像関係を利用して求める（ステップS１１）。楕円を決めるパラメータは長径、短径、傾き角度、中心座標の５つである。このうち、基準楕円Ｅ_０と観測楕円Ｅｃの形状については、前述の方法で既に求めている。現時点では両楕円の中心座標は同一と考えており、また、基準楕円Ｅ_０を基準に座標系を取っているため、この段階では、観測楕円Ｅｃの傾き角度だけが未知である。このため、観測楕円Ｅｃの傾きを変えて、図７の写像が成立する切断角φｃを求める。この観測楕円Ｅｃの傾き角度を変えながら探索する方法について、図９を参照して説明する。

基準楕円Ｅ_０を、図９に示されるような角度に固定する。次に、観測楕円Ｅｃの角度を変化させながら、下記の手順で、写像関係を満足する角度を探索する。まず、基準楕円Ｅ_０と観測楕円Ｅ_０との中心を揃え、観測楕円Ｅ_０を任意の角度に傾ける。次に、基準楕円Ｅ_０と観測楕円Ｅ_０との交点を求める。この交点は、０組、１組または２組の場合がある。次に、交点ＡＢを直線で結び、中心Ｏから直線ＡＢの垂線を引き、垂線と基準楕円Ｅ_０、観測楕円Ｅ_０との交点をそれぞれｍ３，ｍ４とする。次に、線分ＡＢ上で、点Ｏ、点Ａ、点Ｂの近傍を除く、任意の点Ｏαを取り、上記と同様に、交点ｍ３，ｍ４を取る。Ｏ・ｍ１／Ｏ・ｍ２とＯα・ｍ３／Ｏα・ｍ４との比率を比較し、これが一致すれば、現在の角度は写像関係を満たす。

切断角φｃは、図９に示される直線ＡＢと基準楕円Ｅ_０の短径との成す角であるため、写像関係が成立した状態での交点座標から求めることができる。また、写像係数Ｋもこの過程で同時に求まる。上述の探索手順において、観測楕円Ｅｃの推定誤差や探索可能な角度が有限であることを考えると、比率を比較する点は多いほどよい。

（回転角計算）
横断面の回転角φｒは、写像係数Ｋから以下の式（１）で求められる（ステップＳ１２）。

φｒ=ａｃｏｓ（１／Ｋ） ・・・（１）

（姿勢角計算）
ここまでに切断角φｃと回転角φｒは得られている。ただし、前述したように切断角φｃと回転角φｒからヨー角θｙとピッチ角θｐを直接的には解くことができない。このため、図６のＹ軸上に単位ベクトル［０，１，０］を取り、これを切断角φｃで決まる交線ＡＢを回転軸とした回転角φｒの回転変換を行い、得られた変換後のベクトルの空間座標［Ｘｎ，Ｙｎ，Ｚｎ］から、次の式（２）でヨー角θｙとピッチ角θｐを求める。

図１０には、基準楕円Ｅ_０と観測楕円Ｅｃの空間的な関係を示し、図１１には、には、Ｙ軸方向から見た基準楕円Ｅ_０と切断角φｃと回転角φｒとに基づいてＸ−Ｚ平面上に回転させた観測楕円Ｅｃを示し、図１２には、センサ座標系（観測楕円ＥｃとＸ−Ｙ平面との交線の角度の基準として、レーザースキャナから見る）で見た場合の観測楕円Ｅｃを示す。

今、図１１に示す２つの楕円Ｅ_０，Ｅｃの配置関係は求められている。一方、図１２はロール角θｒの定義から、ロール角θｒがゼロの場合での見かけの観測楕円Ｅｃである。つまり、レーザースキャナ１３から得られた観測楕円Ｅｃと、図１２の観測楕円Ｅｃとの角度の差からロール角θｒを求めることができる。図１１、図１２では、同じ形状の楕円を（楕円に対して）同じ方向から見ているため、形状は同じで傾きのみが異なる。このため、図１１の∠Ｂ・Ｏ・Ｆαと図１２の∠Ｂ・Ｏ・Ｆは同じ角度であり、両者の角度の差は図１１における∠Ｄ１・Ｏ・Ｆαである。このため、ロール角θｒを求める手順としては以下に示す（１）〜（４）の手順となる。

（１） 図１１での観測楕円Ｅｃの傾きは既知である。
（２） ∠Ｄ１・Ｏ・Ｆαの大きさを求める（方法は後述する。）
（３） （１）と（２）とにより、図１２の観測楕円Ｅｃの傾きが決まる。
（４） （３）で求めた図１２での観測楕円Ｅｃの傾きと、計測データ（観測断面データ）から推定された観測楕円Ｅｃの傾きとの差がロール角θｒである。

次に、∠Ｄ１・Ｏ・Ｆαの大きさを求め方について説明する。
図１１の楕円配置において、
（１） 基準楕円Ｅ_０とＸ軸との交点Ｄ１を通り、直線ＡＢと直交する直線を求める。
（２） （１）で求め直線と観測楕円Ｅｃαとの交点を求める。
（３） （２）で求めた交点のうち、直線ＡＢで２つに分けられた領域のうち、Ｄ１と同じ側にある交点がＦαとなる。
（４） 交点Ｆαの座標を［Ｘｆ，Ｚｆ］とすると、∠Ｄ１・Ｏ・Ｆα＝tan^-1(Ｘｆ，Ｚｆ)となる。
以上より、ロール角θｒを求めることができる（ステップＳ１３）。

（相対位置計算）
次に、閉鎖空間Ｓの横断面上、すなわち走行ルートＲに直交する方向での搬送車両３Ａの相対位置、すなわちレーザースキャナ１３の相対位置（以下、「センサ位置」という）を求める（ステップＳ１４）。

（１）センサ座標系での座標変換
計測データから推定される観測楕円Ｅｃの中心の座標Ｐｓ（Ｘｓ，Ｙｓ）とすると、この座標はレーザースキャナ１３を基準とする座標系で表したものである。ここで観測楕円Ｅｃの中心座標Ｐｓを基準とした座標系を取ると、レーザースキャナ１３の位置は、Ｐｓ’（−Ｘｓ，−Ｙｓ）と表すことができる。

一方、ロール角θｒについては、前述の方法で既に求めているので、センサ座標系におけるレーザースキャナ１３位置Ｐ_０（Ｘ_０，Ｙ_０）は、次式（３）に示す回転変換より求めることができる。なお、上記のセンサ座標系とは、図１３に示す観測楕円Ｅｃの面内で、Ｏを原点、ＯＦをＸ軸とした座標系である。

（２）空間座標変換
図１３には、空間座標変換の模式図を示す。今、求められたセンサ位置Ｐ_０（Ｘ_０，Ｙ_０）は楕円Ｅｃ内でのＯＦを角度の基準とした座標系である。これに対して、求めたいのはＰ_０の空間座標Ｐｐ（図中のＸＹＺの座標軸）、厳密に言えば、空間座標Ｐｐの内のＸとＺ座標である。Ｐｐは、図中のＥｃとＥｃαが回転軸ＡＢで回転した関係にあることを利用して、以下の手順で求める。

（２−１） Ｐ_０からＥｃα上のＰαの空間座標を求める。
図１３において、Ｆαは観測楕円Ｅｃ上のＦを回転軸ＡＢで回転写像したＥｃα上の写像点である。また、Ｅｃαは、観測楕円Ec を回転軸AB でＸ−Ｚ平面に回転したものなので、Ｅｃα面内でＸ軸をＯ・Ｆαとする座標系を取ると、このときのＰαの座標も（Ｘ_０，Ｙ_０）となる。一方、Ｘ軸とＯ・Ｆαが成す角度は、図１１の∠Ｄ１・Ｏ・Ｆαと同じであり、ロール角θｒの算出時に求めている。以上より、Ｘ−Ｚ座標系におけるＰαの座標は回転変換により求めることができる。

（２−２） 求めたＰαに対して、回転軸ＡＢを軸とした回転角φｒの回転変換を行う。
ここで、Ｐαの回転変換、すなわち回転軸ＡＢ周りに回転角φｒだけの回転変換は、四元数を用いると次式（４）で表される。ここで、求める変換後の点をＰｐ（Ｘｐ，Ｙｐ，Ｚｐ）とすると、最終的には断面内でのセンサ位置は（Ｘｐ,Ｚｐ）となる。

相対位置の計算まで終了すると姿勢角及び相対位置の計算処理（図４参照）は終了し、後続の補正制御を実行する（ステップＳ５）。ここで走行制御部１５ｂは、上述の計算処理で求められた姿勢角や相対位置に基づき、駆動装置１１を介して操舵制御や車輪５ａの駆動制御を行い、搬送車両３Ａを自動運転する。本実施形態に係る誘導方法において、ステップＳ１は走査ステップに相当し、ステップ２〜ステップ５は、走行制御ステップに相当する。

本実施形態では、観測断面データに基づいて楕円が推定された場合を例示したが、観測断面データに基づいて推定された形状が真円となった場合、上記の方法における交線ＡＢの傾き角φｅを定義できないため、ロール角φｒを求めることはできない。ただし、観測断面データに基づいて推定される横断面形状が円となるのは２方向だけであり（表裏を含めると４方向）、搬送車両３Ａの場合は直前の姿勢角と相対位置からレーザースキャナ１３の向きは一意に特定できる。また、搬送車両３Ａの走行制御では、ロール角φｒは必ずしも必要ではないので、実用上支障はない。また、実際の計算処理においては、観測楕円Ｅｃのパラメータを監視することで、この状態を検出できる。

なお、観測断面データに基づく横断面形状が円形の場合であっても、レーザースキャナ１３の姿勢角や相対位置の求め方は楕円の場合と同様であるが、円の半径は観測楕円Ｅｃの短軸の径と一致するため、円の半径情報は未知で構わない。ただし、楕円と異なり、円は回転させても同じ円となるため、観測断面データに基づく横断面形状も横断面の回転角（Ｘ_０，Ｙ_０）φｒが同じであれば、切断角φｃによらず、全て同じ形状となる。逆に言えば、観測断面データに基づく横断面形状から切断角φｃを求めることができない。このため、レーザースキャナ１３の姿勢角と相対位置を一意に求めるためには、ロール角θｒの基準が別途必要となる。例えば、搬送車両３Ａの走行面が平坦であれば、計測データから、または、他のセンサで路面の向きを検知する、あるいは、他のセンサで重力方向を検知することでロール角θｒの基準を取って、レーザースキャナ１３の姿勢角と相対位置を求めることができる。また、基準断面データに基づく横断面形状が円形の場合も同様であり、レーザースキャナ１３の姿勢角と相対位置を一意に求めるためには、ロール角θｒの基準が別途必要となる。

この誘導システム１Ａでは、閉鎖空間Ｓ内に付帯設備を設けるのではなくトンネル軸Ｌ（閉鎖空間Ｓの軸線）に沿った走行ルートＲ上の搬送車両３Ａを基準にしてレーザースキャナ１３で取得され得る基準断面データを記憶している。そして、レーザースキャナ１３で取得された観測楕円Ｅｃと基準楕円Ｅ_０との幾何学的関係から搬送車両３Ａの姿勢角と相対位置との推定値を求め、その推定値に基づいて搬送車両３Ａの走行制御を行うことで走行ルートＲに沿った安定した誘導を実現できる。その結果として、搬送車両３Ａの誘導に要する設備負担を抑え、閉鎖空間Ｓ内での搬送車両３Ａの安定した誘導を実現できる。

特に、誘導システム１Ａでは、レーザースキャナ１３によって横断面形状データを取得している。レーザースキャナ１３であれば、閉鎖空間Ｓの内径（幅）、すなわち走行ルートＲから坑道Ｔの内壁Ｔａまでの距離に関係なく観測誤差は一定である。従って、閉鎖空間Ｓの広狭の影響を受け難いため、特に内壁ｔａから離れた状態で搬送車両３Ａを誘導する必要がある場合に有効である。

（第２実施形態）
本発明の第２実施形態に係る誘導システムについて、図１４、図１５及び図１６を参照して説明する。図１４は、第２実施形態に係る搬送車両の正面図であり、図１５は、第２実施形態に係る誘導システムのブロック図であり、図１６は、第２実施形態に係るレーザースキャナで取得される観測楕円と基準楕円との幾何学的関係を示す図である。なお、第２実施形態について、第１実施形態と実質的に同様の構成については、第１実施形態と同一の符号を示して詳細説明を省略する。

本実施形態に係る搬送車両３Ｂは、第１実施形態に係る搬送車両３Ａとは異なり、お互いの相対角度と相対位置が既知（予め規定済み）である複数のレーザースキャナ１７，１９を備える。なお、本実施形態では、二台のレーザースキャナ１７，１９を搭載しているが、三台以上であってもよい。第１のレーザースキャナ１７及び第２のレーザースキャナ１９は、それぞれ独立して観測断面データを取得する。

制御装置２１の走行制御部２１ｂは、第１及び第２のレーザースキャナ１７，１９それぞれから観測断面データを受け付けると、各観測断面データに基づくそれぞれの観測楕円を推定するための処理を実行する。その結果、走行制御部２１ｂでは、レーザースキャナ１７，１９の数に相当する二つの観測楕円を取得することができる。また、断面形状記憶部２１ａには、第１のレーザースキャナ１７及び第２のレーザースキャナ１９それぞれに対応付けて基準断面データが記憶されている。ここで、レーザースキャナ１７，１９間の相対角度と相対位置とが既知であるので、観測楕円同士の間の空間的な配置を求めることができ、このことを利用して横断面内における各レーザースキャナ１７，１９の姿勢角と相対位置を求める。さらに、走行制御部２１ｂは、求めた姿勢角や相対位置に基づいて、搬送車両３Ｂの走行制御を実行する。以下、本実施形態での、搬送車両３Ｂの姿勢角と相対位置との導出方法について説明する。

走行制御部２１ｂは、レーザースキャナ１７，１９それぞれで取得された観測断面データに基づいて、各観測楕円を推定する。各観測楕円の中心は、基準楕円Ｅ_０の中心に一致すると仮定できるため、各観測楕円の中心を結ぶベクトルは閉鎖空間Ｓの軸方向、すなわちトンネル軸Ｌ方向を示すベクトルとなる。この関係から、図１６に示される幾何学的関係が成立する。ここで、図１６は、観測楕円Ｅｃ１とトンネル軸Ｌとの幾何学的関係を示す図である。

図１６に示されるように、各観測楕円のうち、例えば、一方の観測楕円Ｅｃ１は、基準楕円Ｅ_０との交線ＡＢを回転軸として、ある回転角だけ回転した平面として示すことができる。前述のように、各観測楕円の中心を示す座標からトンネル軸Ｌ方向を示すベクトルを簡単に求めることができ、さらに、トンネル軸Ｌ方向を示すベクトルＳ１・Ｓ２を観測面に投影したベクトルＯＨは必ず交線ＡＢと直交することが解っている。この性質を利用すれば、切断角φｃを直接求めることができる。第１実施形態に係る誘導システム１Ａでは、切断角φｃを求めるために探索が必要であったが、本実施形態では、その探索のための処理が不要になって演算処理に伴う負担が軽減される。切断角φｃが求まれば、第１実施形態と同様にして搬送車両３Ｂの姿勢角と相対位置とを導出する。

なお、本実施形態に係る誘導システム１Ｂでは、搭載位置や向きを変えてレーザースキャナ１７，１９を搬送車両３Ｂに設置しており、両方の観測楕円が円になってしまうことがないようにしている。従って、第１実施形態に係る誘導システム１Ａで生じていた問題、すなわち観測断面データに基づく横断面形状が真円であった場合の問題は、両方の観測断面データのうち、いずれか一方に基づく横断面形状は楕円となるために解消できる。

また、ピッチ角θｐやヨー角θｙがトンネル軸に対して９０度、あるいは９０度の近傍になった場合、観測楕円の長軸の径が無限大または無限大に近くなり（実際にはレーザースキャナの測定範囲を超過するため）、１台のレーザースキャナのみでは解くことができなくなってしまう。実際の計算処理では、計測データの距離、観測楕円のパラメータなどを監視することで、この状態を検出できる。本実施形態では、搭載位置や向きを変えてレーザースキャナ１７，１９を搬送車両３Ｂに設置しており、レーザースキャナ１７，１９の両方共が、ピッチ角θｐやヨー角θｙがトンネル軸に対して９０度とならないようにしている。

この誘導システム１Ｂでは、第１実施形態に係る誘導システム１Ａと同様に、走行ルートＲに沿った搬送車両３Ｂの安定した誘導を実現できる。その結果として、搬送車両３Ｂの誘導に要する設備負担を抑え、閉鎖空間Ｓ内での搬送車両３Ｂの安定した誘導を実現できる。

さらに、この誘導システム１Ｂでは、搭載位置または向きが異なるように搬送車両３Ｂに搭載された第１及び第２のレーザースキャナ１７，１９を備えている。第１及び第２のレーザースキャナ１７，１９は、それぞれ独立して観測断面データを取得し、断面形状記憶部２１ａには、第１及び第２のレーザースキャナ１７，１９それぞれに対応付けて複数の基準断面データが記憶されている。走行制御部２１ｂは、第１及び第２のレーザースキャナ１７，１９それぞれで取得された観測断面データに基づく観測楕円と、レーザースキャナ１７，１９それぞれに対応して記憶されている各基準断面データに基づく基準楕円との幾何学的関係から、閉鎖空間Ｓの横断面上での搬送車両３Ｂの姿勢角及び相対位置の推定値を求め、その推定値に基づいて搬送車両３Ｂの走行制御を行っている。従って、例えば、第１のレーザースキャナ１７で取得された観測断面データに不具合があっても、第２のレーザースキャナ１９で取得された観測断面データに基づく観測楕円から搬送車両３Ｂの姿勢角や相対位置の精度の高い推定値求めることができ、誤検出を防止または低減することができる。

次に、複数台のレーザースキャナ１７,１９を用いて位置と姿勢とを推定する他の方法について、図１７を参照して説明する。図１７は、基準楕円Ｅｏと複数の観測楕円Ｅｃ２，Ｅｃ３との空間的な関係を示す図である。なお、複数台のレーザースキャナ１７,１９の配置と車両方向は既知であると仮定する。

レーザースキャナ１７,１９で計測、推定された複数の観測楕円Ｅｃ２，Ｅｃ３の位置は搬送車両３Ｂを基準とした座標系で求めることができる。得られた観測楕円Ｅｃ２，Ｅｃ３の中心Ｏ２，Ｏ３を結んだ直線Ｏ２・Ｏ３はトンネル軸Ｌを示すため、車両方向ＯＶとトンネル軸Ｓ１・Ｓ２との相対的な関係が定まる。ただし、この段階ではロール角θｒがわからないため、この相対角度を満たす搬送車両３Ｂの位置は一意ではない。このとき、走路が平面であり（例えば、図のＸＹ平面）、ロール角θｒ変動を考慮しなくてもよい場合には、断面内における搬送車両３Ｂの位置と姿勢を直ちに求めることができる。ただし、この場合でも条件を満たす複数の位置と姿勢が存在するが、搬送車両の位置と姿勢の連続性を考慮すれば、正しい方を選択することは容易である。

次にロール角θｒが不明な走行環境において必要なロール角θｒの算出手順を説明する。もちろん、上述の方法によって基準楕円Ｅ_０内における搬送車両３Ｂの位置と姿勢を推定することは可能であるが、ここでは別の方法について説明する。

上述したように、観測楕円Ｅｃ２とＥｃ３とトンネル軸Ｓ１・Ｓ２の空間的な関係はわかっている。また、観測楕円Ｅｃ２，Ｅｃ３をトンネル軸Ｓ１・Ｓ２と直交する平面（図のＸＹ平面）に投影したものは基準楕円Ｅ_０そのものである。このため、観測楕円Ｅｃ２，Ｅｃ３から投影されて得られる基準楕円Ｅ_０の傾きからロール角θｒを求めることができる。数値解析的には、観測楕円Ｅｃ２，Ｅｃ３上の適当な数の点を選び、これらの点に対してトンネル軸Ｓ１・Ｓ２への投影に相当する座標変換を行い、変換後の点群から改めて楕円推定をすることで投影楕円のパラメータを求めることができ、その傾きからロール角θｒを得ることができる。このようにしてロール角θｒが定まれば、容易に車両の位置と姿勢を求めることができる。ただし、ただし、この場合でも条件を満たす複数の位置と姿勢が存在するが、車両の位置と姿勢の連続性を考慮すれば、正しい方を選択することは容易である。

（第３実施形態）
本発明の第３実施形態に係る誘導システムについて、図１８及び図１９を参照して説明する。図１８は、第３実施形態に係る搬送車両の側面図であり、図１９は、第３実施形態に係る誘導システムのブロック図である。なお、本実施形態について、第１実施形態または第２実施形態と実質的に同様の構成については、第１実施形態と同一の符号を示して詳細説明を省略する。図１６は、本実施形態に係る搬送車両の側面図である。

搬送車両３Ｃには、走路認識用のレーザースキャナ２３と障害物検知用のレーザースキャナ２５とが設置されている。走路認識用のレーザースキャナ２３は、搬送車両３Ｃの前方の斜め上方に向けられており、障害物検知用のレーザースキャナ２５は、搬送車両３Ｃの前方の斜め下方に向けられている。走路認識用のレーザースキャナ２３と障害物検知用のレーザースキャナ２５とは、同一構造のレーザースキャナを使用することができる。

走路認識用のレーザースキャナ２３は、観測楕円Ｅｃを推定するための観測断面データを取得し、走行制御部２７ｂに出力する。走行制御部２７ｂでは、レーザースキャナ２３から受け付けた観測断面データから観測楕円を推定し、さらに、断面形状記憶部２７ａに記憶されている基準断面データに基づいて基準楕円を取得する。さらに、走行制御部２７ｂは、観測楕円と基準楕円との幾何学的関係に基づいて搬送車両３Ｃの姿勢角及び相対位置の推定値を求め、その推定値から搬送車両３Ｃの相対誘導を実行する。また、障害物検知用のレーザースキャナ２５は、走行ルートＲとして水平な平面路を検出する。走行制御部２７ｂは、障害物検知用のレーザースキャナ２５で取得した二次元データに基づいて走行ルートＲ上に障害物などが存在すると判断する場合には、搬送車両３Ｃの走行を停止させるような制御を実行する。

この誘導システム１Ｃでは、第１及び第２実施形態に係る誘導システム１Ａ，１Ｂと同様に、走行ルートＲに沿った搬送車両３Ｂの安定した誘導を実現できる。その結果として、搬送車両３Ｂの誘導に要する設備負担を抑え、閉鎖空間Ｓ内での搬送車両３Ｂの安定した誘導を実現できる。

以上、本発明を各実施形態に基づいて説明したが、本発明はこれらの実施形態のみに限定されない。例えば、上記の実施形態では、移動体としての搬送車両３Ａ，３Ｂ，３Ｃの前方を向くレーザースキャナのみを例示したが、後方を向いたレーザースキャナを設置しても良い。

本発明の第１実施形態に係る誘導システムを利用した相対誘導を模式的に示す図である。 本実施形態に係る搬送車両の側面図である。 本実施形態に係る誘導システムのブロック図である。 本実施形態に係る誘導システムを利用して実行される相対誘導の動作手順を示すフローチャートである。 姿勢角、相対位置計算処理の動作手順を示すフローチャートである。 観測楕円と基準楕円とを示す図である。 交線ＡＢを回転軸にして基準楕円Ｅ_０を回転角φｒだけ回転させ、観測楕円Ｅｃに基準楕円Ｅ_０を重ねた状態を示す図である。 図７に示す基準楕円Ｅ_０と観測楕円Ｅｃとの関係を観測楕円Ｅｃの基準軸（長軸と短軸）を示すように書き直した図である。 相対角度の探索のために、観測楕円Ｅｃに基準楕円Ｅ_０を重ねて示す図である。 基準楕円Ｅ_０と観測楕円Ｅｃとの空間的な関係を示す図である。 Ｙ軸方向から見た基準楕円Ｅ_０と切断角φｃと回転角φｒとに基づいてＸ−Ｚ平面上に回転させた観測楕円Ｅｃを示す図である。 センサ座標系（観測楕円とＸ−Ｙ平面との交線の角度を基準として、レーザースキャナから見る）で見た場合の観測楕円を示す図である。 空間座標変換の模式図を示す図である。 本発明の第２実施形態に係る搬送車両の正面図である。 本実施形態に係る誘導システムのブロック図である。 本実施形態に係るレーザースキャナで取得される観測楕円と基準楕円との幾何学的関係を示す図である。 基準楕円Ｅ_０と複数の観測楕円Ｅｃ１，Ｅｃ２との空間的な関係を示す図である。 本発明の第３実施形態に係る搬送車両の側面図である。 本実施形態に係る誘導システムのブロック図である。

符号の説明

１Ａ，１Ｂ，１Ｃ…誘導システム、３Ａ，３Ｂ，３Ｃ…搬送車両（移動体）、１３，１７，１９，２３…レーザースキャナ（走査手段）、１５ａ，２１ａ，２７ａ…断面形状記憶部（断面形状記憶手段）、１５ｂ，２１ｂ，２７ｂ…走行制御部（走行制御手段）、Ｅｏ…基準楕円（基準断面形状）、Ｅｃ，Ｅｃ１，Ｅｃ２，Ｅｃ３…観測楕円（閉鎖空間を横断する断面形状）、Ｌ…トンネル軸（閉鎖空間の軸線）、Ｓ…閉鎖空間、Ｔａ…坑道の内壁（壁面）。

Claims (4)

1. 縦長の閉鎖空間内で、前記閉鎖空間の軸線に沿って移動体を誘導する誘導システムにおいて、
   前記移動体の走行方向の前方及び後方の少なくとも一方を向いて前記移動体に搭載されると共に、前記閉鎖空間を横断する断面形状に関する二次元情報を取得する走査手段と、
   前記閉鎖空間の軸線に沿った走行ルート上の移動体を基準にして前記走査手段で取得され得る基準断面形状を記憶する断面形状記憶手段と、
   前記走査手段で取得された前記断面形状と前記断面形状記憶手段に記憶された前記基準断面形状との幾何学的関係から、前記閉鎖空間の横断面上での前記移動体の位置及び姿勢の推定値を求め、前記推定値に基づいて前記移動体の走行制御を行う走行制御手段と、
   を備えることを特徴とする誘導システム。
2. 前記走査手段は、前記移動体に一または複数搭載され、
   前記断面形状記憶手段は、前記走査手段に対応付けて複数の基準断面形状を記憶し、
   走行制御手段は、前記走査手段で取得された前記断面形状と、前記断面形状記憶手段に対応付けられて記憶されている複数の基準断面形状との幾何学的関係から、前記閉鎖空間の横断面上での前記移動体の位置及び姿勢の推定値を求め、前記推定値に基づいて前記移動体の走行制御を行うことを特徴とする請求項１記載の誘導システム。
3. 前記走査手段は、レーザースキャナであることを特徴とする請求項１または２記載の誘導システム。
4. 縦長の閉鎖空間内で、前記閉鎖空間の軸線に沿って移動体を誘導する誘導方法において、
   前記移動体の進行方向の前方及び後方の少なくとも一方における前記閉鎖空間を横断する断面形状に関する二次元情報を取得する走査ステップと、
   前記閉鎖空間の軸線に沿った走行ルート上の移動体を基準にして前記走査手段で取得され得る基準断面形状と、前記走査ステップで取得された前記断面形状とを対比し、前記断面形状と前記基準断面形状との幾何学的関係から、前記閉鎖空間の横断面上での前記移動体の位置及び姿勢の推定値を求め、前記推定値に基づいて前記移動体の走行制御を行う走行制御ステップと、
   を備えることを特徴とする誘導方法。

Images