(57)【要約】【課題】マーカをよく認識できなかった場合でも、さらに探索して容易且つ迅速にマーカを認識して、自律走行装置の位置を精度よく推定可能にする。【解決手段】操作部１１から入力される初期位置と向き及びゴール地点の情報と、自律走行中にレーザレンジファインダ１３、超音波センサ２３等の検知手段に検知される周囲の情報とにより、ＰＣ１０が移動方向と移動速度を決定し、駆動・ＩＯ制御部２０がモータ２４を駆動制御する。自律走行したい場所を予め手動操作で走行させ、ＰＣ１０が検知手段によって周囲の情報を検知し、地図情報を作成して記憶する。自律走行時には、少なくともゴール地点の近傍の画像データをカメラ１２で取得してマーカを検出し、自己位置を算出する。マーカを検出できない場合は、地図情報からマーカがある可能性が高い領域を特定し、その領域に対して画像の拡大処理を行って再びマーカの検知を行う。【選択図】    図１
2017120551.tif 000002
【特許請求の範囲】
【請求項１】
操作者が情報を入力するための入力手段と、
周囲の情報を検知する検知手段と、
モータの駆動によって移動するための駆動手段と、
前記入力手段によって入力される初期位置及び向きとゴール地点の各情報と、自律走行中に前記検知手段によって検知される周囲の情報とに基づいて、移動方向と移動速度を決定し、前記モータを駆動制御する制御手段と、
自律走行したい場所を予め手動操作で走行させる手動走行手段と、
該手動走行手段による走行中に前記検知手段によって周囲の情報を検知し、地図情報を作成して記憶する地図情報作成・記憶手段と、
を有し、
前記検知手段は、少なくとも前記ゴール地点の近傍の画像データを取得するカメラを有し、
前記制御手段が、前記カメラによって取得した画像データによって、予め決められたマーカを検出して自己の位置を算出する機能を有する自律走行装置であって、
前記制御手段が、前記マーカを検出できない場合は、前記地図情報から前記マーカがある可能性が高い領域に対してマーカの画素を検索して前記マーカがありそうな領域を特定し、該領域に対して前記画像データによる画像の拡大処理を行って、再び前記マーカの検知を行うことを特徴とする自律走行装置。
【請求項２】
前記制御手段が、前記拡大処理を行って前記マーカを検知した場合には、該マーカまでの位置情報の計算を、該拡大処理に用いた倍率を考慮して行うことを特徴とする請求項１に記載の自律走行装置。
【請求項３】
前記制御手段は、前記地図情報に基づいて地図上の前記ゴール地点を認識しておき、該ゴール地点付近に到着した場合にのみ、前記カメラによって取得した画像データによって、前記マーカを検出して自己の位置を算出し、前記ゴール地点まで正確に走行させて停止させる制御を行い、前記マーカを検出できない場合は、前記地図情報から前記マーカがある可能性が高い領域に対してマーカの画素を検索して前記マーカがありそうな領域を特定し、該領域に対して前記画像データによる画像の拡大処理を行って、再び前記マーカの検知を行うことを実行することを特徴とする請求項1又は２に記載の自律走行装置。
【請求項４】
前記検知手段は、レーザレンジファインダと超音波センサの少なくとも一方を備えていることを特徴とする請求項１から３のいずれか一項に記載の自律走行装置。
【請求項５】
前記制御手段が、前記拡大処理を行っても前記マーカを検知できなかった場合は、前記カメラを用いたマーカ検知から、前記レーザレンジファインダ又は超音波センサを用いた周囲情報の検知に切り替える手段を有することを特徴とする請求項４に記載の自律走行装置。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、自律走行装置に関する。
【背景技術】
【０００２】
目的地を指定するとその場所まで自律して走行する自律走行装置が、例えば無人搬送機等に使用されている。その自律走行装置においては、カメラを設置してマーカ検知を行い、数ｃｍ単位で自己位置を推定して高精度な位置合わせをする技術が既に知られている。
【０００３】
例えば、特許文献１には、カメラの座標から位置を推定する目的で、一定の決められた図形をマーカとして用い、自律走行装置に装着されているカメラでそのマーカを撮影する技術が開示されている。そして、そのマーカを撮影した画像データを解析して、カメラの座標を求めることによって自律走行装置の位置を推定する。
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、このような従来の自律走行装置におけるマーカ検知では、マーカを構成する図形を認識できなかった場合は認識しないという問題があった。また、認識精度を上げるために、さらに図形を検出する画像探索をしようとすると、全探索になって処理に時間がかかるという問題があった。
【０００５】
この発明は上記の問題を解決するためになされたものであり、マーカをよく認識できなかった場合でも、さらに探索して容易且つ迅速にマーカを認識できるようにし、自律走行装置の位置を精度よく推定可能にすることを目的とする。
【課題を解決するための手段】
【０００６】
この発明による自律走行装置は、上記の目的を達成するため、操作者が情報を入力するための入力手段と、周囲の情報を検知する検知手段と、モータの駆動によって移動するための駆動手段と、上記入力手段によって入力される初期位置及び向きとゴール地点の各情報と、自律走行中に上記検知手段によって検知される周囲の情報とに基づいて、移動方向と移動速度を決定し、上記モータを駆動制御する制御手段と、自律走行したい場所を予め手動操作で走行させる手動走行手段と、その手動走行手段による走行中に上記検知手段によって周囲の情報を検知し、地図情報を作成して記憶する地図情報作成・記憶手段とを有する。
そして、上記検知手段は、少なくとも上記ゴール地点の近傍の画像データを取得するカメラを有し、上記制御手段が、上記カメラによって取得した画像データによって、予め決められたマーカを検出して自己の位置を算出する機能を有する自律走行装置であって、
上記制御手段が、上記マーカを検出できない場合は、上記地図情報から上記マーカがある可能性が高い領域に対してマーカの画素を検索して上記マーカがありそうな領域を特定し、その領域に対して上記画像データによる画像の拡大処理を行って、再び上記マーカの検知を行うことを特徴とする。
【発明の効果】
【０００７】
この発明によれば、マーカをよく認識できなかった場合でも、さらに探索して容易且つ迅速にマーカを認識することができ、自律走行装置の位置を精度よく推定することが可能になる。
【図面の簡単な説明】
【０００８】
【図１】この発明による自律走行装置の一実施形態の構成を示すブロック図である。
【図２】図１におけるＰＣ１０側のＣＰＵによって実行されるこの発明に関わる処理の第１実施例の流れを示すフローチャートである。
【図３】同じくこの発明に関わる処理の第２実施例の流れを示すフローチャートである。
【図４】同じくこの発明に関わる処理の第３実施例の流れを示すフローチャートである。
【図５】第１〜第３実施例に用いたマーカの図形例を示す図である。
【図６】マーカのコーナ位置検出について説明するための図である。
【図７】各実施例で用いた地図情報の一例を示す図である。
【図８】図７の地図情報にマーカの位置情報を上書き保存し、自律走行装置の初期位置及び向きとゴール地点の各情報を入力することを説明するための図である。
【図９】図５に示したマーカを認識するためのマーカ対応座標点の例を示す図である。
【発明を実施するための形態】
【０００９】
以下、この発明を実施するための形態を図面に基づいて具体的に説明する。
図１は、この発明による自律走行装置の一実施形態の構成を示すブロック図である。
この自律走行装置３０は、パーソナルコンピュータ（以下「ＰＣ」と略称する）１０と、駆動回路とＩＯ制御回路を構成する駆動・ＩＯ制御部２０とが、互いに信号及び情報の遣り取りが可能に接続されている。
【００１０】
そのＰＣ１０には操作部１１を備えると共に、カメラ１２とレーザレンジファインダ１３を搭載している。操作部１１は、操作者が情報を入力するための入力手段であり、カメラ１２とレーザレンジファインダ１３は、周囲の情報を検知する検知手段である。
そのカメラ１２は、少なくともゴール地点の近傍の画像データを取得する。レーザレンジファインダ１３は、レーザの送受信を行って目的物までの正確な距離を測定する小型の距離測定器（レーザ距離計）である。
駆動・ＩＯ制御部２０には、電源であるバッテリ２１と複数の超音波センサ２３、複数のモータ２４及びそれと同数のエンコーダ２５が、給電線や信号線で接続されている。
【００１１】
複数のモータ２４とその駆動力伝達機構及び車輪や操舵機構が、モータの駆動によって移動するための移動手段である。複数の超音波センサ２３も、周囲の情報を検知する検知手段である。
ＰＣ１０は、操作部１１からの操作者の入力をトリガに、手動操作による走行中に地図情報を作成し、その地図情報を記憶（保存）する。そして、自律走行時には、ＰＣ１０がその地図情報を用いて移動ルート及び移動速度を決定して、駆動・ＩＯ制御部２０に対して駆動要求する。
【００１２】
ＰＣ１０はまた、手動操作による地図情報の作成および自律走行時の障害物検知のため、駆動・ＩＯ制御部２０を介して全ての超音波センサ２３によって、自律走行装置の周辺情報をセンシングする。これらの処理はＰＣ１０が保有するＣＰＵによってソフトウェア処理で実行する。
駆動・ＩＯ制御部２０は、ＰＣ１０からの移動ルートと移動速度の情報を受けて、複数のモータ２４を駆動制御して車輪の回転や操舵等を実行し、実際に動作した値を複数のエンコーダ２５によって検知してＰＣ１０にフィードバックする。
【００１３】
また、駆動・ＩＯ制御部２０は、複数の超音波センサ２３に対して周辺情報のセンシング要求を行なう。その結果、駆動・ＩＯ制御部２０が各超音波センサ２３から得る周辺情報を、ＰＣ１０にフィードバックする。
【００１４】
予期しない障害物の接近に対しては、ＰＣ１０からのルート情報に従って駆動・ＩＯ制御部２０がモータ２４を駆動制御する前に、ＰＣ１０から超音波センサ２３に周辺情報のセンシングを一定間隔で要求して、周辺情報を把握している。そのため、ＰＣ１０側から駆動・ＩＯ制御部２０側に、モータ２４の減速指示や停止指示を直接行って、障害物との衝突回避を行なう。
【００１５】
駆動・ＩＯ制御部２０もＣＰＵを有しており、ＰＣ１０側の処理と独立して制御するため、ＰＣ１０側の処理にＣＰＵ処理が専有されてモータ２４の駆動制御が待たされることはない。
バッテリ２１は、各モジュールを動作させるために電気を供給する。
【００１６】
マーカを撮像して画像で取り込むためのカメラ１２はＰＣ１０に接続されており、リアルタイムに、例えば６４０×４８０画素の画像を３０ＦＰＳ（フレーム／秒）で、ＲＧＢ画像として取得する。
この自律走行装置が、目的地周辺の高精度な位置検知が必要な場所であるゴール地点の近傍に到達した場合、ＰＣ１０がカメラ１２から画像を取得する。そして、ＰＣ１０側のＣＰＵでその画像に対してマーカ検知処理を行ってルート決定を行ない、マーカがある目的地に向かって移動する。
【００１７】
この自律走行装置３０においては、ＰＣ１０と駆動・ＩＯ制御部２０とが、制御手段と、手動走行手段と、地図情報作成・記憶手段の機能を果している。
その制御手段としての機能では、入力手段である操作部１１によって入力される初期位置及び向きとゴール地点の各情報と、自律走行中に検知手段によって検知される周囲の情報とに基づいて、移動方向と移動速度を決定し、モータ２４を駆動制御する。
手動走行手段としての機能は、自律走行したい場所を予め手動操作で走行させるための機能である。
【００１８】
ＰＣ１０と駆動・ＩＯ制御部２０による地図情報作成・記憶手段としての機能では、手動走行手段による走行中に検知手段によって周囲の情報を検知し、地図情報を作成して内部の不揮発性メモリに記憶する。
ＰＣ１０と駆動・ＩＯ制御部２０による制御手段としての機能はまた、カメラ１２によって取得した画像データによって、予め決められたマーカを検出して自己の位置を算出する。しかし、その制御手段の機能でマーカを検出できない場合は、記憶した地図情報からマーカがある可能性が高い領域に対してマーカの画素を検索してマーカがありそうな領域を特定する。そして、その領域に対して上記画像データによる画像の拡大処理を行って、再びマーカの検知を行う。
【００１９】
以下に、いくつかの実施例を説明していくが、その自律走行装置のハード構成は、基本的に図１に示した構成と共通であり、ＰＣ１０内のＣＰＵによってソフトウェア処理されるルート決定や地図生成の処理内容が異なる例である。
その第１実施例を図２によって説明する。図２は、カメラ１２から入力される画像（静止画像）に対して、ＰＣ１０側のＣＰＵによってなされる、この発明に関わる処理の第１実施例の流れを示すフローチャートである。
【００２０】
この第１実施例及び後に説明する第２、第３実施例でも、図５に示すように、正方形の黒枠内の白背景中に複数の黒の正方形の点がある図形（パターン）をマーカとして用いる。そして、カメラ１２から入力される画像上にそのマーカを構成すると思われる黒画素の連続があるかどうか検索する。
図２のフローチャートにおいて、網点を施した部分すなわちステップＳ８とＳ１０の処理が、この発明による特徴的な処理であり、それ以外の処理はマーカ検知の一般的な処理である。
【００２１】
ＰＣ１０のＣＰＵが図２のフローチャートに示す処理を開始すると、まずステップＳ１で、カメラ１２から取得した画像データに対して四角形抽出の処理を行う。この四角形抽出では、取得した画像データの各画素を単純２値化し、その２値画像データに対して輪郭抽出して、その輪郭の直線近似を行なう。そして、コーナが４つあるかどうかによる四角形の特徴判断を行って、マーカを構成する四角形（正方形の黒枠及びその内部の黒の正方形の点）を抽出する。
ＣＰＵは次のステップＳ２で、マーカを構成する四角形を検知したか否かを判断し、検知したと判断した場合は、ステップＳ３で黒枠内に四角形が２個以上あるか否かを判断する。その結果、２個以上あると判断すると、ＣＰＵはステップＳ４へ進んで透視投影変換を行う。
【００２２】
その透視投影変換では、ＣＰＵは抽出した四角形のコーナ４点を用いて透視変換を行ない、黒の正方形をマーカが画像正面に来るように画像変換する。
その後、ＣＰＵはステップＳ５でマーカの方向確認を行う。そのため、黒枠内の正方形の点に相当すると思われる黒画素の塊について、１００×１００画素に正規化を行う。そして、その正規化した画像に対して左上隅（０°）、右上隅（９０°）、右下隅（１８０°）、左下隅（２７０°）に分割して、その各分割画像の黒画素数をカウントする。
その結果、黒画素数のカウント値が最も多い分割画像の方向をマーカが向いている方向と判断する。
【００２３】
次に、ＣＰＵはステップＳ６でコーナ位置検知を行って、マーカを構成する各正方形のコーナ位置を高精度化する。それは、公知のcvFindCornerSubPix関数を用いて、サブピクセル精度のコーナあるいは鞍点を検出するために、繰り返し処理を行うことである。サブピクセル精度のコーナ位置決めは、近傍領域の中心からその領域内に位置する点に向かう各ベクトルが画像勾配と直交する原理に基づいて、探索窓の中心を新しい値に再設定し、値の変化量が与えられた閾値内に収まるようになるまで繰り返し計算を行う。
【００２４】
その後ＣＰＵは、ステップＳ７で３次元−２次元の対応点から物体（ここではカメラ１２）の姿勢算出を行う。すなわち、後述するカメラ行列や歪み係数から、物体上の点座標とそれに対応する画像上の投影点の集合が与えられた場合、公知のsolvePnP関数を用いてカメラ座標（ｘ，ｙ，θ）を推低する。
カメラ座標は、再投影誤差を最小にするように求める。つまり、観測された投影点と物体座標空間における物体上の点座標の配列を投影した点との距離の二乗和を最小にして求める。また、後述する回転ベクトルと並進ベクトルもsolvePnP関数を用いて求める。
【００２５】
ソース的に記載すると、次のようになる。
cv::solvePnP(worldPoints,imagePoints,intrinsicMatrix,distortionCoeffs,rotationVector,translationVector)
worldPoints：正方形の黒枠と内部の正方形のコーナの両方の座標点(マーカによる固定値)
imagePoints：コーナ検出から得られた座標点
intrinsicMatrix：カメラ内部パラメータ(あらかじめ求める値)
distortionCoeffs：歪み系数(あらかじめ求める値)
rotationVector：回転ベクトル
translationVector：並進ベクトル
【００２６】
そして、ＣＰＵは最後のステップＳ８で、拡大処理の有無を考慮したカメラの自己位置推定情報算出を行った後、図２の処理を終了する。このステップＳ８では、ステップＳ７における物体の姿勢算出で求めた回転ベクトルと並進ベクトルから、カメラ座標（ｘ，ｙ，θ）を求める。θはＹＡＷ角（ヨー角）、すなわち上下軸まわりの回転角である。
また、マーカを検知するために拡大処理を行ったかどうか判断し、拡大処理を行なっている場合は、拡大処理に用いた倍率を考慮して、カメラ座標（ｘ，ｙ，θ）を算出する。
このカメラ座標が、自律走行装置に搭載しているカメラ１２の座標情報であり、マーカまでの位置情報にも対応する。したがって、このカメラ座標（ｘ，ｙ，θ）を拡大処理に用いた倍率を考慮して算出することが、マーカまでの位置情報を拡大処理に用いた倍率を考慮して計算することになる。
【００２７】
ＰＣ１０のＣＰＵは、前述したステップＳ２でマーカを構成する四角形を検知したと判断できなかった場合、およびステップＳ３で黒枠内に四角形が２個以上あると判断できなかった場合は、マーカを検知できなかった場合である。その場合は、ステップＳ９へ進んで拡大処理をしたか否かを判断する。その結果、拡大処理をしたと判断しなかった（拡大処理していないと判断した）場合は、ステップＳ１０へ進んで、拡大処理を行う。
【００２８】
その拡大処理では、記憶している地図情報から、マーカの座標情報（ｘ，ｙ，θ）を参照して、マーカがある可能性が高い領域に対してマーカの画素を検索してマーカがありそうな領域を特定する。そして、その特定した領域に対してカメラによって取得した画像データによる画像の拡大処理を行なう。この拡大処理の際に、四角形抽出がしやすくなるように、コーナの先鋭化やエッジ強調も行なうとよい。
その後、再びステップＳ１〜Ｓ３でマーカの検知を行う。それでもマーカを検知できず、ステップＳ９へ進んで拡大処理をしたと判断した場合は、ステップＳ１１へ進んでマーカなしと認識して、処理を終了する。
【００２９】
この発明は、マーカを検知できなかった場合にマーカの検知精度を上げるため、予め作成した地図情報にマーカの位置情報を保存しておき、効率よくマーカがある可能性が高い領域に対して画像の拡大処理を行う。
たとえば、カメラ１２によってマーカが全部撮像されていても、距離が遠いためマーカを構成する画素数が少なくてマーカを検知できない場合がある。そのような場合に、マーカがある可能性が高い領域を効率的に拡大処理してマーカを検知可能にすることができる。それによって、カメラ１２の位置を確実に算出して、自律走行装置の自己位置を精度よく推定することができる。
このように、マーカを認識できなかった場合でも、拡大処理後にさらに探索して容易且つ迅速にマーカを認識し、自律走行装置の位置を精度よく推定することができる。
【００３０】
図３は、カメラ１２から入力される画像（静止画像）に対して、ＰＣ１０側のＣＰＵによってなされるこの発明に関わる処理の第２実施例の流れを示すフローチャートである。
この第２実施例の図３に示すフローチャートの基本的な流れは、図２に示した第１実施例と同じであるが、カメラからの画像データを用いてマーカ検知を行うことを、おおよそのゴール地点に到着した場合にのみ行うようにしている点だけが相違する。
【００３１】
マーカはゴール地点付近にしか無いことと、自律走行装置がゴール地点に向かう途中にマーカに似た図形がある場合に、それを誤検知してゴールと認識しないようにするため、この第２実施例ではマーカ検知をする場所をゴール地点付近に限定している。
そのため、図３に示すフローチャートでは、ＰＣ１０側のＣＰＵが処理を開始すると、まずステップＳ０で地図情報によるルートに従い、おおよそのゴール地点に到着したか否かを判断する。その結果、おおよそのゴール地点に到着したと判断するまでは何もせずに処理を終了する。これを一定の時間間隔で繰り返し、ステップＳ０でおおよそのゴール地点に到着したと判断すると、ステップＳ１の四角形抽出以降の処理に進む。
ステップＳ１〜Ｓ１１の各処理は、図２によって説明した実施例１と同じであるから説明を省略する。
【００３２】
このように、この第２実施例では、ＰＣ１０側のＣＰＵが予め記憶した地図情報に基づいて地図上のゴール地点を認識しておき、自律走行装置がゴール地点付近に到着した場合にのみ、第１実施例と同じ処理を実行する。すなわち、カメラ１２によって取得した画像データによって、予め決められたマーカを検出して自己の位置を算出する。しかし、そのマーカを検出できない場合は、上記地図情報からマーカがある可能性が高い領域に対してマーカの画素を検索してマーカがありそうな領域を特定する。そして、その領域に対して画像データによる画像の拡大処理を行って、再びマーカの検知を行うことを実行する。
【００３３】
したがって、マーカを正確に検出してカメラ１２の位置を算出し、自律走行装置の自己位置を精度よく推定でき、ゴール地点まで正確に走行させて停止させる制御を行うことができる。また、自律走行装置がゴール地点に向かう途中にマーカに似た図形があった場合でも、それを誤検知してゴールと認識する恐れがなくなる。
【００３４】
図４は、カメラから入力される画像（静止画像）に対して、ＰＣ１０側のＣＰＵによってなされるこの発明に関わる処理の第３実施例の流れを示すフローチャートである。
この第３実施例の図４に示すフローチャートの基本的な流れは、図３に示した第２実施例と同じである。しかし、ゴール地点付近でマーカがあるだろうと予想した方向に拡大処理を行ってマーカ検知を行なっても、マーカを検知できなかった場合は、別の検知方法に切り替えるようにした点が相違する。
【００３５】
この第３実施例では、ＰＣ１０のＣＰＵがステップＳ１０で画像の拡大処理を行って、ステップＳ１〜Ｓ３で再度マーカ検知を行なってもマーカを検知できなかった場合には、ステップＳ１１でマーカなしと認識した後、ステップＳ１２での処理を行う。そのステップＳ１２では、カメラ１２を用いたマーカ検知からレーザレンジファインダ（ＬＲＦ）１３を用いた周囲情報の検知に切り替える。そして、この処理を終了する。
レーザレンジファインダ１３による周囲情報の検知は、図１に示したレーザレンジファインダ１３を用いた平面の形状認識により、ゴールの形状を予め保持しておき、その形状を使って、レーザレンジファインダ１３がその反射波を検知した方向に近づくように制御する。
【００３６】
カメラを用いたマーカ検知は１秒間に数フレームの画像を取得できる。そのため、あるフレームでマーカを検知できなくてレーザレンジファインダ１３を用いた周囲情報の検知に切替え、方向を決めた次のフレームで、再びカメラを用いたマーカ検知を行ってゴールに近づく方法も考えられる。
図１に示した自律走行装置は超音波センサ２３も備えているので、レーザレンジファインダ（ＬＲＦ）１３を用いた周囲情報の検知に代えて、超音波センサ２３を用いた周囲情報の検知に切り替えてもよい。
【００３７】
あるいは、レーザレンジファインダ（ＬＲＦ）１３と超音波センサ２３を用いた周囲情報の検知に切り替えてもよい。レーザレンジファインダ（ＬＲＦ）１３と超音波センサ２３の一方又は両方を使用して、ＰＣ１０が周辺の障害物までの距離を算出して走行経路を決定することができる。
また、図１に示した自律走行装置は、レーザレンジファインダ（ＬＲＦ）１３と超音波センサ２３とを備えているが、そのいずれか一方のみを備えるようにしてもよい。
図４におけるステップＳ０は図３によって説明した実施例２と同じであり、ステップＳ１〜Ｓ１１は図２によって説明した実施例１と同じであるから説明を省略する。
このようにすれば、カメラによって取得した画像データではマーカを検知できなかった場合でも、自律走行装置がゴール地点へ精度よく到達できる可能性が高まる。
【００３８】
図５は、第１〜第３実施例に用いたマーカの図形例を示す。このマーカの図形は、白（背景）と黒の２値図形であり、黒を構成する外枠の中に複数個の黒の小さい正方形が存在する。図２〜図４のフローチャートに示したように、このマーカを構成する複数個の黒の正方形の四角形を抽出し、マーカを認識した後、透視変換を行って、このマーカを撮影したカメラ１２の位置及び角度を示すカメラ座標（ｘ，ｙ，θ）を予測する。
図６によってコーナの位置検出について説明する。コーナ位置決めは、図６に示す近傍領域の中心ｑから、その領域内に位置する点ｐに向かう各ベクトルが、点ｐにおける画像勾配と直交するという考えに基づいて算出する。それは、数１で表現できる。
【００３９】
【数１】
2017120551.tif 000003
ここでＤＩｐｉは、近傍領域ｑ内の点ｐｉにおける画像勾配である。ｑは、このεｉを最小にする値として求められる。εｉを０とすることで、数２との連立方程式が成り立つ。
【００４０】
【数２】
2017120551.tif 000004
ここで、画像勾配はｑの近傍領域での総和をとられる。１次勾配をＧ、２次勾配をｂとすると、ｑとの間に数３の関係が成り立つ。
なお、数１及び数２における「Ｔ］は転置行列であることを示している。
【００４１】
【数３】
2017120551.tif 000005
図６に示した探索窓の中心をこの新しい値ｑに再設定し、値の変化量が与えられた閾値内に収まるようになるまで繰り返し計算を行なって、マーカを構成する黒の正方形のコーナの位置（座標）を求める。
【００４２】
図２〜図４におけるステップＳ７の「３次元−２次元の対応点から物体の姿勢算出」の処理で、カメラの位置は、３次元位置が既知の点Ｐとその画像上での観測座標ｐの複数の対応から求めることができる。この問題はPerspective-n-Pont  Problem（ＰｎＰ問題）として知られている。
３次元位置が既知の点の配列とその画像上での観測座標の配列、カメラの内部パラメータ行列、歪み係数ベクトルをそれぞれ入力とし、カメラの外部パラメータ（回転ベクトルと並進ベクトル）を推定する。
カメラの内部パラメータ行列と歪み係数ベクトルは、カメラごとに予めキャリブレーション処理を行うことによって求めることができるパラメータであり、数４にその例を示す。
【００４３】
【数４】
2017120551.tif 000006
【００４４】
カメラ行列は、カメラ固有にもつカメラ内部パラメータ行列で、３×３のベクトルで表わせる。
歪み係数ベクトルは４、５または８個の要素を持つベクトルである。このベクトルがＮＵＬＬ又は空である場合、歪み係数は０であると見なされる。
【００４５】
カメラの外部パラメータ（回転ベクトルと並進ベクトル）から、カメラ位置（ｘ，ｙ，θ）を求める。
回転ベクトルと並進ベクトルは以下の様なベクトルで算出される。
回転ベクトル＝（ｒ１，ｒ２，ｒ３）
並進ベクトル＝（ｈ１，ｈ２，ｈ３）
【００４６】
ｘ＝ｈ２／１０００［ｍｍ］
ｙ＝ｈ１／１０００［ｍｍ］
θは、従来技術と同じクォータニオン（四元数）を回転ベクトルのｒ２を用いて求めて、ＹＡＷ角（ヨー角）に変換する。
ｘ，ｙにおいては、拡大処理を行わないでマーカを検知すれば上記の式から求められ、拡大処理を行った場合、その拡大率に対して予め用意した係数を掛けて求める。θは拡大処理に無関係なため、拡大処理の有無によって計算式が変わらない。
ｘ＝ｈ２＊ＫＸ／１０００［ｍｍ］    ＫＸ：拡大率に応じた係数
ｙ＝ｈ１＊ＫＹ／１０００［ｍｍ］    ＫＹ：拡大率に応じた係数
【００４７】
図７に前述した各実施例で用いた地図情報の例を示す。これは、自律走行装置が自律走行する上で必要な地図情報の例になる。この地図情報は、図７に示すように２次元上に表すことができる。
自律走行装置３０を手動走行手段によってマニュアル（手動）走行させる。その手動走行中に、図１に示したＰＣ１０がレーザレンジファインダ１３を用いて、レーザレンジファインダ１３が設置されている平面上の障害物までの距離をスキャンする。それによって周囲の情報を検知し、自律走行装置３０が走行可能な箇所（図７における白領域）と走行不可能な箇所（灰色領域）を識別して、地図情報として作成する。その地図情報をＰＣ１０がメモリに記憶する。
【００４８】
初期状態では全て走行不可能な箇所（灰色領域）のみの画像であるが、自律走行装置３０のマニュアル走行の際に、レーザレンジファインダ１３のスキャンにより、レーザ光が周辺の障害物に当たって戻ってくる時間によって距離を測定する。その情報によって、自律走行装置３０が走行可能な箇所（白領域）を広げて地図情報を作成していく。
この地図情報に対して、図８に示すように複数のマーカの位置情報（ｘ，ｙ，θ）を上書きで保存しておく。
【００４９】
図８は、図７の地図情報にマーカの位置情報を上書き保存し、自律走行装置３０の初期位置及び向きとゴール地点の各情報を入力することを説明するための図である。マーカＡ〜Ｃの位置情報を次のように表わす。
マーカＡ（ｘ１，ｙ１，θ１）
マーカＢ（ｘ２，ｙ２，θ２）
マーカＣ（ｘ３，ｙ３，θ３）
さらに他のマーカＸがある場合の位置情報は、次のように表わす。
マーカＸ（ｘｘ，ｙｘ，θｘ）
【００５０】
これらの各マーカは、自律走行装置の選択可能な複数のゴール地点から所定距離の箇所にそれぞれ設置される。
上記マーカの位置情報（ｘ，ｙ，θ）は、ある原点からの予め設置しているマーカの座標情報(ｘ,ｙ,θ)であり、マーカがある数とマーカごとの原点からの座標が決まっているので、ＰＣ１０のＲＯＭに保存しておくことができる。したがって、各マーカのパターンは、全て例えば図５に示したような同じパターンでよい。
【００５１】
ここで、マーカのパターン判別情報の例を説明する。例えば図５に示したマーカの場合、図９に示すように、黒い正方形の外枠とその内側の６個の小さい黒い正方形のパターンのうちの左下の１個の各コーナの座標点を認識する。
そのため、図９に丸付き数字１〜１２で示す各コーナの座標点を、マーカ対応座標点（ｘ，ｙ，ｚ）の情報として、例えば表１に示すようにＰＣ１０のＲＯＭに格納しておく。
この各コーナのマーカ対応座標点（ｘ，ｙ，ｚ）は、丸付き数字１のマーカ対応座標点を原点（０，０，０）としている。
【００５２】
【表１】
2017120551.tif 000007
【００５３】
ＰＣ１０が検知したパターンにおけるこの１２箇所のコーナの座標点が、記憶している各マーカ対応座標点と一致すれば、所定のマーカであると認識できる。
その各マーカは、ｚ軸座標が０であるから、ｚ軸はカメラと同じ高さを想定しており、自律走行装置３０が持つカメラ１２と同じ高さで、壁や柱に貼られているものを想定している。
【００５４】
自律走行時には、図８に示すように、この地図上に自律走行装置３０の初期位置及び向いている方向Ｆを入力し、自律走行させたいゴール地点Ｇを設定して自律走行を要求する。それによって、自律走行装置３０は、内蔵のＰＣ１０が地図情報と自己位置情報によりルートを決めて、ゴール地点まで自律走行する。
マーカの座標情報(ｘ,ｙ,θ)は、地図上にあるどのマーカに近づいたのか把握するために用いる。カメラ座標（ｘ,ｙ,θ）は、最も近づいたマーカをカメラが画像で捉えて、マーカ検出処理を行ってマーカとして認識できた時、最も近づいたマーカとの相対座標をカメラから取得した画像データから算出して求める。
【００５５】
マーカを利用するのは、自律走行装置３０が自律走行してゴール地点付近のマーカに近づき、設定されたマーカと対向するゴール地点に高精度（誤差数ｃｍ）に停止する際である。自律走行装置３０が搭載しているカメラによってマーカがある可能性が高い領域を撮像し、その画像データに基づいてＰＣ１０がマーカ検知処理を行う。ＰＣ１０がマーカを検知すると、その検知したマーカとの距離をカメラ座標から判断して、そのマーカの方向へ移動するように駆動・ＩＯ制御部２０を制御する。
【００５６】
なお、この実施形態では、複数のマーカを地図情報に対して予め設定した位置情報によって識別するようにした。しかし、複数のマーカを識別する他の方法として、各マーカごとに正方形の黒枠の内側の小さい黒い正方形の大きさ、個数、配置等を変えておき、自律走行装置がそれを判別することによってマーカを識別することも可能である。
【００５７】
また、自律走行装置がゴール地点付近でマーカ検知する場合に限らず、自律走行中に通過あるは立ち寄るべき要所ごとにマーカを検知する場合にも、この発明を適用することができる。それによって、マーカをよく認識できなかった場合でも、さらに探索して容易且つ迅速にマーカを認識することができ、自律走行装置の位置を精度よく推定することが可能になる。
【００５８】
以上、この発明の実施形態について説明してきたが、その実施形態の各部の具体的な構成や処理の内容等は、そこに記載したものに限るものではない。
また、この発明は上述した実施形態に限定されるものではなく、特許請求の範囲の各請求項に記載された技術的特徴を有する以外は、何ら限定されないことは言うまでもない。
さらに、以上説明してきた実施形態の構成例、動作例及び変形例等は、適宜変更又は追加したり一部を削除してもよく、相互に矛盾しない限り任意に組み合わせて実施することも可能である。
【符号の説明】
【００５９】
１０：パーソナルコンピュータ（ＰＣ）    １１：操作部    １２：カメラ    
１３：レーザレンジファインダ    ２０：駆動・ＩＯ制御部    
２１：バッテリ    ２３：超音波センサ    ２４：モータ    
２５：エンコーダ    ３０：自律走行装置
【先行技術文献】
【特許文献】
【００６０】
【特許文献１】特開２０１４−２１６２４号公報
【図１】
2017120551.tif 000008
【図２】
2017120551.tif 000009
【図３】
2017120551.tif 000010
【図４】
2017120551.tif 000011
【図５】
2017120551.tif 000012
【図６】
2017120551.tif 000013
【図７】
2017120551.tif 000014
【図８】
2017120551.tif 000015
【図９】
2017120551.tif 000016