移動ロボットの遠方環境認識装置及び方法
【課題】小型かつ安価な装置を用いて、高速走行時に検出が必要な検出必要距離内に存在する平面及びある程度小さな障害物も検出することができ、かつ走行路に対して前方の走行可能領域が傾いており、検出必要距離内に存在する障害物を検出できない場合でも、走行安全性を確保することができる移動ロボットの遠方環境認識装置及び方法を提供する。
【解決手段】移動ロボット１０に走行経路の生成に必要な環境地図データ５を出力する遠方環境認識装置。移動ロボット１０に固定して配置され前方の平面領域を含む相互に重複する領域の画像を撮像する一対の車載カメラ２２と、移動ロボットに固定して配置され前方正面及び所定の検出必要距離に相当する下向き角度でレーザ光を前方左右に走査しその反射光から障害物の位置を検出するレーザセンサ２４と、車載カメラとレーザセンサの出力から単一の座標系に定義された平面領域１及び障害物４を含む環境地図データ５を生成するコンピュータ２６とを備える。
【解決手段】移動ロボット１０に走行経路の生成に必要な環境地図データ５を出力する遠方環境認識装置。移動ロボット１０に固定して配置され前方の平面領域を含む相互に重複する領域の画像を撮像する一対の車載カメラ２２と、移動ロボットに固定して配置され前方正面及び所定の検出必要距離に相当する下向き角度でレーザ光を前方左右に走査しその反射光から障害物の位置を検出するレーザセンサ２４と、車載カメラとレーザセンサの出力から単一の座標系に定義された平面領域１及び障害物４を含む環境地図データ５を生成するコンピュータ２６とを備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、例えば、人がＧＰＳ座標系でウェイポイントを入力し、これを通過するように自律的に走行経路の生成を行って走行する自律移動ロボット、又は半自律移動ロボット及び遠隔で操縦する操縦支援装置に係り、特に移動ロボットの遠方環境認識装置及び方法に関する。
なお、半自律移動ロボットとは、自律走行と遠隔操縦とを混成して走行させるロボットである。
【背景技術】
【０００２】
移動ロボットの遠方環境認識方法として、車載カメラからのステレオ画像を利用して、走行可能な平面領域及び障害物を検出する技術が、特許文献１〜３に既に開示されている。
【０００３】
また、進行方向の斜め下方に向けて測距センサを取り付け、かつ同センサを進行方向に向けて取り付けることで、走行可能領域及び障害物を検出する技術が、特許文献４に既に開示されている。
【０００４】
図１は特許文献１〜３の技術が適用される車両の説明図である。
この図において、車両５０（移動体）のウィンドシールド上部の左右には、車両５０が走行する道路平面領域を含む画像を撮像する一対の撮像手段である基準カメラ５２及び参照カメラ５４が固定配置される。基準カメラ５２及び参照カメラ５４は、共通の撮像領域が設定されたＣＣＤカメラ等からなるステレオカメラであり、これらには、撮像した画像を処理することで道路平面領域と障害物となる可能性のある物体とを検出する演算処理装置５６が接続される。なお、以下の説明では、基準カメラ５２によって撮像された画像を「基準画像」と称し、参照カメラ５４によって撮像された画像を「参照画像」と称する。
【０００５】
図２は一対の撮像手段間での２次元射影変換を説明するための模式図であり、
図３は平面領域を抽出する手順の説明図である。
先ず、特許文献１〜３における道路平面領域の抽出原理を説明する。
【０００６】
図２に示すように、空間中にある平面Π上の観測点Ｍ１が基準画像Ｉｂ及び参照画像Ｉｒに投影されるとき、基準画像Ｉｂ上での同次座標をｍｂ、参照画像Ｉｒ上での同次座標をｍｒとすると、各同次座標ｍｂ、ｍｒは、２次元射影変換行列Ｈによって関係付けられる。
【０００７】
ここで、Ｈは３×３の射影変換行列である。また、図２において、Ｏは基準カメラ５２の光学中心、Ｏ’は参照カメラ５４の光学中心、Ｒは基準カメラ座標系から参照カメラ座標系への回転行列、ｔは、参照カメラ座標系において参照カメラ５４の光学中心から基準カメラ５２の光学中心へ向かう並進ベクトル、ｄは基準カメラ５２の光学中心Ｏと平面Πとの距離、ｎは平面Πの法線ベクトルである。
【０００８】
道路等の走行可能な領域は、空間中でほぼ平面であると見なすことができれば、図３（Ａ）の参照画像Ｉｒに対して適切な射影変換を施すことにより、平面の部分に対して図３（Ｂ）（Ｃ）の基準画像Ｉｂに一致するような画像（図３（Ｃ））を得ることができる。
一方、平面上にない物体は、射影変換した際に画像（図３（Ｃ）と図３（Ｄ））が一致しないため、その物体の領域は走行可能な領域でない、あるいは、障害物となり得る領域であると判断することができる。したがって、図３（Ｅ）のように、平面の拡がりを算出することにより、走行可能な平面領域及び障害物となる物体の検出が可能となる。
【０００９】
上述した特許文献１〜３の手段により、車載カメラからのステレオ画像を利用して、走行可能な前方の平面領域、基準カメラ５２の光学中心Ｏと平面領域との距離ｄ、前方の平面領域の法線ベクトルｎを検出することができる。
【００１０】
一方、図４は、特許文献４による自律移動車両の側面図である。
この図において自律移動車両６１は、記憶した地図情報に基づいて自律的に移動する車両であって、進行方向の斜め下方に向けて車両前面に取付けられた下方測距センサ６２と、進行方向に向けて車両前面に取付けられた前方距離センサ６３と、車両本体の車輪の回転数から移動量を計測する移動量計測手段（図示せず）とを備え、前方距離センサ６３で取得した情報に下方測距センサ６２で取得した情報を重ね合わせて地図情報を作成し、障害物を検出して回避を行いつつ移動する。
【先行技術文献】
【特許文献】
【００１１】
【特許文献１】特開２００５−２１７８８３公報、「ステレオ画像を用いた道路平面領域並びに障害物検出方法」
【特許文献２】特許第４２７０３８６公報、「移動体移動量算出装置」
【特許文献３】特許第４２９７５０１公報、「移動体周辺監視装置」
【特許文献４】特許第４０５５７０１公報、「自律移動車両」
【発明の概要】
【発明が解決しようとする課題】
【００１２】
移動ロボットにおいて、高速走行（例えば約５０ｋｍ／ｈ）を実現するためには、必要な制動距離及び制御サイクルを考慮すると、例えば４０ｍ程度の遠距離の平面領域及び障害物位置を計測する必要がある。以下、この距離を「検出必要距離」と呼ぶ。
【００１３】
上述した特許文献１〜３の技術では、障害物位置の計測にカメラによるステレオ視を使用するため、遠方（検出必要距離）の縁石や路面上の石などの小さな障害物を検出することは困難である。
【００１４】
一方、特許文献４の技術では、走行可能領域を検出するために、斜め下方に向けて下方測距センサ６２を備えている。しかし、遠方（検出必要距離）の路面の計測を行う場合、下方測距センサ６２の下向き角度θは、水平に対して非常に小さく（例えば３度未満）になるため、路面からの反射が非常に微弱であり、高出力のレーザセンサが必要である。また、レーザセンサのスキャンレートも高く設定する必要がある。そのようなセンサは大型かつ高価なものとなり、移動ロボットへの採用は現実的ではない。
【００１５】
本発明は、上述した問題点を解決するために創案されたものである。すなわち、本発明の第１の目的は、小型かつ安価な装置を用いて、高速走行時に検出が必要な検出必要距離内に存在する平面及びある程度小さな障害物も検出することができる移動ロボットの遠方環境認識装置及び方法を提供することにある。また、本発明の第２の目的は、走行路に対して前方の走行可能領域が傾いており、検出必要距離に存在する障害物を検出できない場合でも、走行安全性を確保することができる移動ロボットの遠方環境認識装置及び方法を提供することにある。
【課題を解決するための手段】
【００１６】
本発明によれば、移動ロボットに、走行経路の生成に必要な環境地図データを出力する遠方環境認識装置であって、
前記移動ロボットに固定して配置され、前方の平面領域を含む相互に重複する領域の画像を撮像する一対の車載カメラと、
前記移動ロボットに固定して配置され、レーザ光を前方左右に走査しその反射光から障害物の位置を検出するレーザセンサと、
前記車載カメラとレーザセンサの出力から、単一の座標系に定義された平面領域及び障害物を含む前記環境地図データを生成するコンピュータと、を備えたことを特徴とする遠方環境認識装置が提供される。
【００１７】
本発明の実施形態によれば、前記移動ロボットは、自律的に走行経路の生成を行って走行する自律移動ロボット、半自律移動ロボット、又は遠隔で操縦する操縦支援装置である。
また、前記レーザセンサは、複数のスキャンラインを持つレーザセンサである。
【００１８】
また本発明によれば、移動ロボットに、走行経路の生成に必要な環境地図データを出力する遠方環境認識方法であって、
前記移動ロボットに固定して配置された一対の車載カメラにより、前方の平面領域を含む相互に重複する領域の画像を撮像し、
前記移動ロボットに固定して配置されたレーザセンサにより、レーザ光を前方左右に走査しその反射光から障害物の位置を検出し、
前記画像から前記平面領域と該平面領域のカメラ座標系における法線ベクトルとを検出し、
前記平面領域、平面領域の法線ベクトル、障害物の位置から単一の座標系に定義された平面領域及び障害物を含む前記環境地図データを生成する、ことを特徴とする遠方環境認識方法が提供される。
【００１９】
本発明の実施形態によれば、路面と正対物の再帰反射率の違いに着眼し、前記レーザセンサの反射光の受光強度により障害物の有無を検出する。
さらに、遠方の計測であることから、障害物位置に対応する地図セルに投票して、地図上を探索窓によって探索し、探索窓内の投票値が閾値を超える場合に、その窓内のセルを障害物として、前記レーザセンサから算出される障害物位置の計測誤差を補正する、ことが好ましい。
【００２０】
また、新たに検出された前方の平面領域のカメラ座標系における法線ベクトルが、カメラ座標系において所定の範囲を超える場合に、前記レーザセンサの走査領域が路面上から外れ、障害物検出が有効ではないとして、新たに検出された前方の平面領域を除外して前記環境地図データを生成する。
また、新たに検出された前方の平面領域のカメラ座標系における法線ベクトルから、走行領域に対する車両の角度を算出し、この角度から、レーザ光が地面上の障害物を検出しうる範囲にある領域を算出し、この領域と重複する平面検出領域を選択して、前記環境地図データに反映する。
【発明の効果】
【００２１】
上記本発明の装置及び方法によれば、一対の車載カメラ、レーザセンサ、及びコンピュータからなる小型かつ安価な装置により、単一の座標系に定義された平面領域及び障害物を含む前記環境地図データを生成するので、この環境地図データを用いて、自律移動ロボットが自律的に走行経路の生成を行って走行することができる。
【００２２】
また、レーザセンサがレーザ光を前方左右に走査しその反射光から障害物の位置を検出するので、高速走行時に検出が必要な検出必要距離に存在する小さな障害物を検出することができる。
【００２３】
また、新たに検出された前方の平面領域のカメラ座標系における法線ベクトルが、カメラ座標系において所定の範囲を超える場合に、新たに検出された前方の平面領域を除外して前記環境地図データを生成するので、車両に対して前方の走行可能領域が傾いており、検出必要距離内に存在する障害物を検出できない場合でも、元の走行可能領域のみが表示されるので、走行安全性を確保することができる
【００２４】
従って、ステレオカメラ（一対の車載カメラ）、レーザセンサの組合せにより、相互の欠点を補うことができ、遠方の環境認識が実現できる。
また小さな障害物が存在する場合でも、遠方（現状約４０ｍ）の環境認識が実現できるため、高速走行（現状約５０ｋｍ／ｈ）が可能となる。
【図面の簡単な説明】
【００２５】
【図１】特許文献１〜３の手段が適用される車両の説明図である。
【図２】一対の撮像手段間での２次元射影変換を説明するための模式図である。
【図３】特許文献１〜３による平面領域を抽出する手順の説明図である。
【図４】特許文献４による自律移動車両の側面図である。
【図５】本発明が適用される自律移動ロボットの説明図である。
【図６】自律移動ロボットのブロック図である。
【図７】自律移動ロボットの使用状態を示す側面図である。
【図８】自律移動ロボットの特殊な使用状態を示す側面図である。
【図９】移動ロボット１０の特殊な使用状態を示す別の説明図である。
【図１０】自己位置評定用コンピュータ、画像処理用コンピュータ、及び環境地図生成用コンピュータの動作フロー図である。
【図１１】車両制御用コンピュータの動作フロー図である。
【図１２】レーザセンサによる障害物検出フロー図である。
【図１３】図１２の説明図である。
【図１４】本発明の遠方環境認識装置から出力する環境地図データの説明図である。
【発明を実施するための形態】
【００２６】
以下、本発明の好ましい実施形態を図面を参照して説明する。なお各図において、共通する部分には同一の符号を付し、重複した説明は省略する。
【００２７】
図５は、本発明が適用される自律移動ロボットの説明図であり、図６は自律移動ロボットのブロック図である。
【００２８】
図５、図６において、自律移動ロボット１０は、ＧＰＳ１１、ジャイロセンサ１２、自己位置評定用コンピュータ１３、車両制御装置１４及び操縦装置１５を備える。
自律移動ロボット１０は、この例では自走する車両であるが、本発明はこれに限定されず、半自律移動ロボットや操縦支援装置であってもよい。なお、以下の説明では、自律移動ロボット１０を、単に「移動ロボット」又は「車両」と呼ぶ。
【００２９】
車両制御装置１４は、この例では経路生成用コンピュータ１４ａ、車両制御用コンピュータ１４ｂ、ドライバ１４ｃ及びモータ、アクチュエータ１４ｄからなり、経路生成用コンピュータ１４ａにより自律的に走行経路の生成を行い、車両制御用コンピュータ１４ｂ、ドライバ１４ｃ及びモータ、アクチュエータ１４ｄにより車両を操縦して走行するようになっている。
操縦装置１５は、この例ではノートパソコンであり、人がノートパソコン１５によりＧＰＳ座標系でウェイポイントを入力し、経路生成用コンピュータ１４ａに無線で通信するようになっている。
またＧＰＳ１１、ジャイロセンサ１２及び自己位置評定用コンピュータ１３により周辺の地図、車両の位置、車両の姿勢を検出し、経路生成用コンピュータ１４ａに出力するようになっている
【００３０】
上述した構成により、人がＧＰＳ座標系でウェイポイントを入力することにより、経路生成用コンピュータ１４ａによりウェイポイントを通過するように移動ロボット１０は自律的に走行路検出及び経路生成を行い車両を操縦して走行することができる。
【００３１】
本発明の遠方環境認識装置２０は、上述した移動ロボット１０に走行経路の生成に必要な環境地図データ５を出力する装置である。
【００３２】
図５、図６において、本発明の遠方環境認識装置２０は、一対の車載カメラ２２、レーザセンサ２４、及びコンピュータ２６を備える。
コンピュータ２６は、この例では画像処理用コンピュータ２６ａと環境地図生成用コンピュータ２６ｂからなり、車載カメラ２２とレーザセンサ２４の出力から、単一の座標系に定義された平面領域及び障害物を含む環境地図データを生成する。
【００３３】
図７は、移動ロボット１０の使用状態を示す側面図である。
この図において、一対の車載カメラ２２は、例えばステレオカメラであり、移動ロボット１０に固定して配置され、前方の平面領域１を含む相互に重複する領域の画像２（ステレオ画像）を撮像する。
画像処理用コンピュータ２６ａは、上述した特許文献１〜３の手段により、車載カメラ２２からのステレオ画像２を利用して、平面領域１と、平面領域１の法線ベクトルｎ（カメラ座標系における法線ベクトル）を検出する。
【００３４】
レーザセンサ２４は、この例では、移動ロボット１０に固定して配置されたレーザセンサである。またこのレーザセンサ２４は、４本のスキャンラインを有しており、前方正面及び所定の検出必要距離に相当する下向き角度でレーザ光３ａ〜３ｄを前方左右に走査しその反射光から障害物４の位置を検出するようになっている。
【００３５】
この例で、レーザ光３ａは、前方正面に水平に照射され、その反射光から、前方に位置する障害物４の位置（方向及び距離）を検出する。また、レーザ光３ｂ〜３ｄは、レーザ光３ａから一定の角度（例えば、０．５〜１．０度）ずつ下向きに傾斜しており、それぞれの反射光から、前方に位置する障害物４の位置（方向及び距離）を検出するようになっている。
【００３６】
また、本発明の方法では、路面と正対物の再帰反射率の違いに着眼し、レーザセンサ２４の反射光の受光強度により障害物の有無を検出する。
すなわち、画像処理用コンピュータ２６ａによる平面検出により検出することができなかった、路面上の小さな障害物をレーザセンサ２４の反射光の受光強度により検出する。
レーザセンサ２４のレーザ光３ｂ〜３ｄは平面領域１に対しては浅い入射角となり、受光強度は弱い。一方、正対する障害物４は深い入射角となり高い受光強度が得られる。本発明の方法では、この差により、障害物４の検出を行う。また、計測が遠距離となるため、レーザセンサから算出される障害物位置にも誤差があるので、これを考慮した処理を行う。
すなわち、遠方の計測であることから、障害物位置に対応する地図セルに投票して、地図上を探索窓によって探索し、探索窓内の投票値が閾値を超える場合に、その窓内のセルを障害物として、レーザセンサから算出される障害物位置の計測誤差を補正する。
【００３７】
環境地図生成用コンピュータ２６ｂは、自己位置評定用コンピュータ１３、車載カメラ２２及びレーザセンサ２４の出力、すなわち、周辺の地図、車両の位置、車両の姿勢、平面領域１、平面領域１の法線ベクトルｎ、及び障害物４の位置から、単一の座標系に定義された平面領域１及び障害物４を含む環境地図データ５を生成する。
この環境地図データ５は、車両制御装置１４の経路生成用コンピュータ１４ａに入力され、経路生成用コンピュータ１４ａにより、自律的に走行路検出及び経路生成を行うために用いられる。
【００３８】
図８は、移動ロボット１０の特殊な使用状態を示す側面図である。
この例では、移動ロボット１０が走行中の路面１ａに対して前方の走行可能領域１が傾いており、検出必要距離内に存在する障害物４を検出できない場合を示している。
すなわち現在走行する路面１ａに対して前方の走行可能領域１が上述したレーザ光３ａ〜３ｄの下向き角度（例えば最大約３度）以上に傾くと、レーザ光３ａ〜３ｄが路面（走行可能領域１）から外れてしまう。
レーザセンサ２４が反射光を検出せず、レーザセンサ２４から測距データが出力されないということは、障害物４が存在しないことを意味する。そのためレーザセンサ２４の走査範囲が、路面から外れる場合は、障害物４は検出されず、このため、障害物発見の遅れとなる問題が発生する。
この問題を回避するために、本発明では、新たに検出された前方の平面領域１のカメラ座標系における法線ベクトルｎが、カメラ座標系において所定の範囲を超える場合に、レーザセンサの走査領域が路面上から外れ、障害物検出が有効ではないとして、新たに検出された前方の平面領域を除外して前記環境地図データ５を生成する。
すなわち、本発明では、移動ロボット１０から見た平面領域１の姿勢角とレーザセンサ２４のレーザ光３ａ〜３ｄの成す角が、想定範囲から外れた場合には、平面検出処理によって新たに検出された前方の平面領域を地図に反映させないという処理をとることで、上記問題を回避する。
この場合、例えば車両１０は元の平面領域内で減速し、前記角度が閾値内に戻りしだい、新たに検出された前方の平面領域を地図に反映させて、高速走行を続行する。
【００３９】
図９は、移動ロボット１０の特殊な使用状態を示す別の説明図である。この図において、（Ａ）は走行領域に対する車両１０の角度が正常な場合、（Ｂ）は車両１０にピッチ角が発生した場合、（Ｃ）は車両１０にロール角が発生した場合である。
図９（Ｂ）の車両１０にピッチ角が発生した場合は、図８の場合に相当する。この場合、レーザ光のスキャン範囲２７の全体において、レーザセンサ２４が反射光を検出せず、レーザ光のスキャンが有効ではない（無効）ため、平面検出結果を反映させない。
【００４０】
一方、図９（Ｃ）の車両１０にロール角が発生した場合は、レーザ光のスキャン範囲２７の一部でレーザが有効であるため、この有効範囲では平面検出結果を反映する。
すなわち車両１０が例えば時計方向にロールしていて、前方は水平な平面であると画像で検出できた場合、レーザセンサ２４ではある角度以上に左側に向いた方向が所定の角度以上に上空を向いてしまい、無効な計測となる。このような場合を想定して、レーザ光のスキャン範囲２７の全部の有効・無効を判断するのではなく、スキャンのうち、有効な扇状のエリアを、計測点の向きと平面の向きから決定する。
この場合、新たに検出された前方の平面領域のカメラ座標系における法線ベクトルから、走行領域に対する車両の角度を算出し、この角度から、レーザ光が地面上の障害物を検出しうる範囲にある領域を算出し、この領域と重複する平面検出領域を選択して、前記環境地図データに反映する。
【００４１】
図１０は、自己位置評定用コンピュータ、画像処理用コンピュータ、及び環境地図生成用コンピュータの動作フロー図であり、Ｓ１〜Ｓ４の各ステップ（工程）を有する。
ノートパソコン１５により、ＧＰＳ座標系でウェイポイントを入力し、車両制御用コンピュータ１４に無線で入力する。
自己位置評定用コンピュータ１３は、ＧＰＳ１１とジャイロセンサ１２からデータを受信し（Ｓ１）、周辺の地図、車両の位置、車両の姿勢を検出し、車両制御用コンピュータ１４に出力する。
画像処理用コンピュータ２６ａは、車載カメラ２２からのステレオ画像２を利用して、平面領域１の地図と、車両から見た平面姿勢角（すなわち平面領域１の法線ベクトルｎ）を検出し（Ｓ２）、環境地図生成用コンピュータ２６ｂと車両制御用コンピュータ１４に出力する。
環境地図生成用コンピュータ２６ｂは、自己位置評定用コンピュータ１３の出力（すなわち、周辺の地図、車両の位置、車両の姿勢）とレーザセンサ２４の出力から、障害物の検出処理を行い（Ｓ３）、障害物マップ４ａを出力する。さらにこの環境地図生成用コンピュータ２６ｂは、障害物マップ４ａと車載カメラ２２の出力（すなわち、平面領域１、平面領域１の法線ベクトルｎ）から、地図データを合成して、単一の座標系に定義された平面領域１及び障害物４を含む環境地図データ５を生成し（Ｓ４）、車両制御用コンピュータ１４に出力する。
【００４２】
図１１は、車両制御用コンピュータの動作フロー図であり、Ｓ１１〜Ｓ１８の各ステップ（工程）を有する。
車両制御用コンピュータ１４は、ノートパソコン１５からウェイポイントを受信し、自己位置評定用コンピュータ１３から車両位置と車両姿勢を受信し、環境地図生成用コンピュータ２６ｂから環境地図データ５を受信し、画像処理用コンピュータ２６ａから車両から見た平面姿勢角を受信する。
車両制御用コンピュータ１４は、ウェイポイント、車両位置、車両姿勢及び環境地図データ５から、ポテンシャル法によりポテンシャル場を生成し（Ｓ１１）、最急降下法により経路を設定する（Ｓ１２）。このポテンシャル法と最急降下法は、車両位置や障害物位置のポテンシャルを高く設定し、ウェイポイントや平面領域のポテンシャルを低く設定して、ポテンシャルの高い方から低い方に向かって水が流れるような方向に経路を設定する手法である。
【００４３】
車両制御用コンピュータ１４は、次いで、旋回半径から車速目標値Ａを設定し（Ｓ１３）、さらに平面姿勢角と車両姿勢から車速目標値Ｂを設定し（Ｓ１４）、車速目標値Ａ，Ｂの小さい方を採用する（Ｓ１５）。
次いで、車両制御用コンピュータ１４は、アクセル、ブレーキ位置の目標値を設定し（Ｓ１６）、旋回半径からステアリング位相の目標値を設定し（Ｓ１７）、モータ、アクチュエータを作動させる（Ｓ１８）。
以上の各ステップを車両制御用コンピュータ１４は、所定の制御間隔Δｔで繰り返し実施し、自律的に走行経路の生成を行い、車両を操縦して走行する。
【００４４】
図１２は、レーザセンサによる障害物検出フロー図であり、Ｓ２１〜Ｓ３０の各ステップ（工程）を有する。また図１３は図１２の説明図である。
図１２において、レーザセンサ２４は、前方正面及び所定の検出必要距離に相当する下向き角度でレーザ光３ａ〜３ｄを前方左右に走査し、その反射光の受光強度が所定の閾値以上であるものから測距値を算出する。この場合、車両の走行に支障がある大きさの障害物を確実に検出するように、レーザ光３ａ〜３ｄの走査密度（すなわち走査レート）を設定する。
検出されたＮ０個のデータ（位相と測距値）を出力し（Ｓ２１）、そのデータから環境認識に必要な位相範囲内のデータＮ個を抽出し（Ｓ２２）、車両姿勢角と自己位置を用いて座標変換によりＮ個の慣性座標特徴（Ｘｅ，Ｙｅ，Ｚｅ）を算出する（Ｓ２３）。この場合、測距データがない場所は、障害物がないとして扱う。
図１３（Ａ）はこの時点（Ｓ２３）での障害物候補マップを示している。この図において、ａは車両位置、ｂは環境認識に必要な位相範囲、ｃは障害物候補である。
【００４５】
次いで、図１２において、障害物候補マップを呼び出し投票作業へ移行し（Ｓ２４）、Ｎ個の障害物候補に対して、座標（Ｘｅ，Ｙｅ）を地図分解能δで除し、投票セル位置（ｉ，ｊ）を算出し（Ｓ２５）、投票セル位置（ｉ，ｊ）に投票する（Ｓ２６）。
次にｉ＝ｉｓ〜ｉｅ、ｊ＝ｊｓ〜ｊｅの環境認識範囲において、探索窓内の投票数の総和を算出し（Ｓ２７）、投票数の総和が所定の閾値以上である場合（Ｓ２８）に、探索窓内の地図セルを障害物に設定する（Ｓ２９）。
図１３（Ｂ）は、探索窓ｄ内の投票数の総和を算出する状態を示している。
【００４６】
次いで、図１２において、障害物候補マップから、環境認識範囲で障害物情報を切り出し、障害物マップとして出力する（Ｓ３０）。図１３（Ｃ）は、障害物マップの例であり、この図でｅは環境認識に必要な範囲、ｆは障害物である。
【００４７】
図１４は、本発明の遠方環境認識装置２０から出力する環境地図データの説明図である。この図において、（Ａ）はある時刻ｔまでに検出した平面領域１ａ、（Ｂ）は時刻ｔに検出した平面領域１、（Ｃ）は時刻ｔに検出した障害物領域ｆを示している。
【００４８】
図１４（Ａ）に示す時刻ｔまでに検出した平面領域１ａは、走行可能領域であり、車両ａは安全に走行できる。
図１４（Ｂ）に示す時刻ｔに検出した平面領域１は、画像処理用コンピュータ２６ａにより車載カメラ２２からのステレオ画像２を利用して検出された平面領域１であり、障害物がない限り車両ａは安全に走行できる。
図１４（Ｃ）に示す時刻ｔに検出した障害物領域ｆは、レーザセンサ２４により上述した方法で検出される。
図１４（Ｃ）の図は、上述した単一の座標系に定義された平面領域１及び障害物４を含む環境地図データ５に相当しており、この環境地図データ５に基づき、経路生成用コンピュータ１４ａにより、自律的に走行路検出及び経路生成を行うことができる。
【００４９】
また、図８に示したように、レーザセンサ２４の走査範囲が、路面から外れる場合は、障害物４は検出されず、このため、障害物発見の遅れとなるが、本発明では、移動ロボット１０から見た平面領域１の姿勢角とレーザセンサ２４のレーザ光３ａ〜３ｄの成す角が、想定範囲から外れた場合には、平面検出処理によって検出された図１４（Ｂ）の平面領域１を地図に反映させないという処理をとることで、図１４（Ａ）に示す平面領域１ａのみを走行可能領域とするので、車両ａは安全に走行することができる。
【００５０】
上述した本発明の装置及び方法によれば、一対の車載カメラ２２、レーザセンサ２４、及びコンピュータ２６ａ，２６ｂからなる小型かつ安価な装置により、単一の座標系に定義された平面領域１及び障害物４を含む環境地図データ５を生成するので、この環境地図データ５を用いて、自律移動ロボット１０が自律的に走行経路の生成を行って走行することができる。
【００５１】
また、レーザセンサ２４が前方正面及び所定の検出必要距離に相当する下向き角度でレーザ光３ａ〜３ｄを前方左右に走査し、その反射光から障害物４の位置を検出するので、高速走行時に検出が必要な検出必要距離内に存在する小さな障害物４を検出することができる。
【００５２】
また、新たに検出された前方の平面領域１の法線ベクトルｎと、走行中の路面１ａの法線ベクトルｎａとの角度が、所定の閾値を超える場合に、新たに検出された前方の平面領域１を除外して環境地図データ５を生成するので、走行中の路面１ａに対して前方の走行可能領域１が傾いており、検出必要距離内に存在する障害物４を検出できない場合でも、元の走行可能領域のみが表示されるので、走行安全性を確保することができる
【００５３】
従って、ステレオカメラ（一対の車載カメラ）、レーザセンサの組合せにより、相互の欠点を補うことができ、遠方の環境認識が実現できる。
また小さな障害物が存在する場合でも、遠方（現状約４０ｍ）の環境認識が実現できるため、高速走行（現状約５０ｋｍ／ｈ）が可能となる。
【００５４】
なお、本発明は上述した実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々に変更することができることは勿論である。
【符号の説明】
【００５５】
１ 平面領域、１ａ 走行中の路面、２ 画像（ステレオ画像）、
３ａ〜３ｄ レーザ光、４ 障害物、５ 環境地図データ、
１０ 自律移動ロボット（移動ロボット、車両）、１１ ＧＰＳ、
１２ ジャイロセンサ、１３ 自己位置評定用コンピュータ、
１４ 車両制御装置、１４ａ 経路生成用コンピュータ、
１４ｂ 車両制御用コンピュータ、１４ｃドライバ、
１４ｄ モータ、アクチュエータ、１５ 操縦装置、
２０ 遠方環境認識装置、２２ 車載カメラ（ステレオカメラ）、
２４ レーザセンサ、
２６ コンピュータ、２６ａ 画像処理用コンピュータ、
２６ｂ 環境地図生成用コンピュータ、２７ スキャン範囲
【技術分野】
【０００１】
本発明は、例えば、人がＧＰＳ座標系でウェイポイントを入力し、これを通過するように自律的に走行経路の生成を行って走行する自律移動ロボット、又は半自律移動ロボット及び遠隔で操縦する操縦支援装置に係り、特に移動ロボットの遠方環境認識装置及び方法に関する。
なお、半自律移動ロボットとは、自律走行と遠隔操縦とを混成して走行させるロボットである。
【背景技術】
【０００２】
移動ロボットの遠方環境認識方法として、車載カメラからのステレオ画像を利用して、走行可能な平面領域及び障害物を検出する技術が、特許文献１〜３に既に開示されている。
【０００３】
また、進行方向の斜め下方に向けて測距センサを取り付け、かつ同センサを進行方向に向けて取り付けることで、走行可能領域及び障害物を検出する技術が、特許文献４に既に開示されている。
【０００４】
図１は特許文献１〜３の技術が適用される車両の説明図である。
この図において、車両５０（移動体）のウィンドシールド上部の左右には、車両５０が走行する道路平面領域を含む画像を撮像する一対の撮像手段である基準カメラ５２及び参照カメラ５４が固定配置される。基準カメラ５２及び参照カメラ５４は、共通の撮像領域が設定されたＣＣＤカメラ等からなるステレオカメラであり、これらには、撮像した画像を処理することで道路平面領域と障害物となる可能性のある物体とを検出する演算処理装置５６が接続される。なお、以下の説明では、基準カメラ５２によって撮像された画像を「基準画像」と称し、参照カメラ５４によって撮像された画像を「参照画像」と称する。
【０００５】
図２は一対の撮像手段間での２次元射影変換を説明するための模式図であり、
図３は平面領域を抽出する手順の説明図である。
先ず、特許文献１〜３における道路平面領域の抽出原理を説明する。
【０００６】
図２に示すように、空間中にある平面Π上の観測点Ｍ１が基準画像Ｉｂ及び参照画像Ｉｒに投影されるとき、基準画像Ｉｂ上での同次座標をｍｂ、参照画像Ｉｒ上での同次座標をｍｒとすると、各同次座標ｍｂ、ｍｒは、２次元射影変換行列Ｈによって関係付けられる。
【０００７】
ここで、Ｈは３×３の射影変換行列である。また、図２において、Ｏは基準カメラ５２の光学中心、Ｏ’は参照カメラ５４の光学中心、Ｒは基準カメラ座標系から参照カメラ座標系への回転行列、ｔは、参照カメラ座標系において参照カメラ５４の光学中心から基準カメラ５２の光学中心へ向かう並進ベクトル、ｄは基準カメラ５２の光学中心Ｏと平面Πとの距離、ｎは平面Πの法線ベクトルである。
【０００８】
道路等の走行可能な領域は、空間中でほぼ平面であると見なすことができれば、図３（Ａ）の参照画像Ｉｒに対して適切な射影変換を施すことにより、平面の部分に対して図３（Ｂ）（Ｃ）の基準画像Ｉｂに一致するような画像（図３（Ｃ））を得ることができる。
一方、平面上にない物体は、射影変換した際に画像（図３（Ｃ）と図３（Ｄ））が一致しないため、その物体の領域は走行可能な領域でない、あるいは、障害物となり得る領域であると判断することができる。したがって、図３（Ｅ）のように、平面の拡がりを算出することにより、走行可能な平面領域及び障害物となる物体の検出が可能となる。
【０００９】
上述した特許文献１〜３の手段により、車載カメラからのステレオ画像を利用して、走行可能な前方の平面領域、基準カメラ５２の光学中心Ｏと平面領域との距離ｄ、前方の平面領域の法線ベクトルｎを検出することができる。
【００１０】
一方、図４は、特許文献４による自律移動車両の側面図である。
この図において自律移動車両６１は、記憶した地図情報に基づいて自律的に移動する車両であって、進行方向の斜め下方に向けて車両前面に取付けられた下方測距センサ６２と、進行方向に向けて車両前面に取付けられた前方距離センサ６３と、車両本体の車輪の回転数から移動量を計測する移動量計測手段（図示せず）とを備え、前方距離センサ６３で取得した情報に下方測距センサ６２で取得した情報を重ね合わせて地図情報を作成し、障害物を検出して回避を行いつつ移動する。
【先行技術文献】
【特許文献】
【００１１】
【特許文献１】特開２００５−２１７８８３公報、「ステレオ画像を用いた道路平面領域並びに障害物検出方法」
【特許文献２】特許第４２７０３８６公報、「移動体移動量算出装置」
【特許文献３】特許第４２９７５０１公報、「移動体周辺監視装置」
【特許文献４】特許第４０５５７０１公報、「自律移動車両」
【発明の概要】
【発明が解決しようとする課題】
【００１２】
移動ロボットにおいて、高速走行（例えば約５０ｋｍ／ｈ）を実現するためには、必要な制動距離及び制御サイクルを考慮すると、例えば４０ｍ程度の遠距離の平面領域及び障害物位置を計測する必要がある。以下、この距離を「検出必要距離」と呼ぶ。
【００１３】
上述した特許文献１〜３の技術では、障害物位置の計測にカメラによるステレオ視を使用するため、遠方（検出必要距離）の縁石や路面上の石などの小さな障害物を検出することは困難である。
【００１４】
一方、特許文献４の技術では、走行可能領域を検出するために、斜め下方に向けて下方測距センサ６２を備えている。しかし、遠方（検出必要距離）の路面の計測を行う場合、下方測距センサ６２の下向き角度θは、水平に対して非常に小さく（例えば３度未満）になるため、路面からの反射が非常に微弱であり、高出力のレーザセンサが必要である。また、レーザセンサのスキャンレートも高く設定する必要がある。そのようなセンサは大型かつ高価なものとなり、移動ロボットへの採用は現実的ではない。
【００１５】
本発明は、上述した問題点を解決するために創案されたものである。すなわち、本発明の第１の目的は、小型かつ安価な装置を用いて、高速走行時に検出が必要な検出必要距離内に存在する平面及びある程度小さな障害物も検出することができる移動ロボットの遠方環境認識装置及び方法を提供することにある。また、本発明の第２の目的は、走行路に対して前方の走行可能領域が傾いており、検出必要距離に存在する障害物を検出できない場合でも、走行安全性を確保することができる移動ロボットの遠方環境認識装置及び方法を提供することにある。
【課題を解決するための手段】
【００１６】
本発明によれば、移動ロボットに、走行経路の生成に必要な環境地図データを出力する遠方環境認識装置であって、
前記移動ロボットに固定して配置され、前方の平面領域を含む相互に重複する領域の画像を撮像する一対の車載カメラと、
前記移動ロボットに固定して配置され、レーザ光を前方左右に走査しその反射光から障害物の位置を検出するレーザセンサと、
前記車載カメラとレーザセンサの出力から、単一の座標系に定義された平面領域及び障害物を含む前記環境地図データを生成するコンピュータと、を備えたことを特徴とする遠方環境認識装置が提供される。
【００１７】
本発明の実施形態によれば、前記移動ロボットは、自律的に走行経路の生成を行って走行する自律移動ロボット、半自律移動ロボット、又は遠隔で操縦する操縦支援装置である。
また、前記レーザセンサは、複数のスキャンラインを持つレーザセンサである。
【００１８】
また本発明によれば、移動ロボットに、走行経路の生成に必要な環境地図データを出力する遠方環境認識方法であって、
前記移動ロボットに固定して配置された一対の車載カメラにより、前方の平面領域を含む相互に重複する領域の画像を撮像し、
前記移動ロボットに固定して配置されたレーザセンサにより、レーザ光を前方左右に走査しその反射光から障害物の位置を検出し、
前記画像から前記平面領域と該平面領域のカメラ座標系における法線ベクトルとを検出し、
前記平面領域、平面領域の法線ベクトル、障害物の位置から単一の座標系に定義された平面領域及び障害物を含む前記環境地図データを生成する、ことを特徴とする遠方環境認識方法が提供される。
【００１９】
本発明の実施形態によれば、路面と正対物の再帰反射率の違いに着眼し、前記レーザセンサの反射光の受光強度により障害物の有無を検出する。
さらに、遠方の計測であることから、障害物位置に対応する地図セルに投票して、地図上を探索窓によって探索し、探索窓内の投票値が閾値を超える場合に、その窓内のセルを障害物として、前記レーザセンサから算出される障害物位置の計測誤差を補正する、ことが好ましい。
【００２０】
また、新たに検出された前方の平面領域のカメラ座標系における法線ベクトルが、カメラ座標系において所定の範囲を超える場合に、前記レーザセンサの走査領域が路面上から外れ、障害物検出が有効ではないとして、新たに検出された前方の平面領域を除外して前記環境地図データを生成する。
また、新たに検出された前方の平面領域のカメラ座標系における法線ベクトルから、走行領域に対する車両の角度を算出し、この角度から、レーザ光が地面上の障害物を検出しうる範囲にある領域を算出し、この領域と重複する平面検出領域を選択して、前記環境地図データに反映する。
【発明の効果】
【００２１】
上記本発明の装置及び方法によれば、一対の車載カメラ、レーザセンサ、及びコンピュータからなる小型かつ安価な装置により、単一の座標系に定義された平面領域及び障害物を含む前記環境地図データを生成するので、この環境地図データを用いて、自律移動ロボットが自律的に走行経路の生成を行って走行することができる。
【００２２】
また、レーザセンサがレーザ光を前方左右に走査しその反射光から障害物の位置を検出するので、高速走行時に検出が必要な検出必要距離に存在する小さな障害物を検出することができる。
【００２３】
また、新たに検出された前方の平面領域のカメラ座標系における法線ベクトルが、カメラ座標系において所定の範囲を超える場合に、新たに検出された前方の平面領域を除外して前記環境地図データを生成するので、車両に対して前方の走行可能領域が傾いており、検出必要距離内に存在する障害物を検出できない場合でも、元の走行可能領域のみが表示されるので、走行安全性を確保することができる
【００２４】
従って、ステレオカメラ（一対の車載カメラ）、レーザセンサの組合せにより、相互の欠点を補うことができ、遠方の環境認識が実現できる。
また小さな障害物が存在する場合でも、遠方（現状約４０ｍ）の環境認識が実現できるため、高速走行（現状約５０ｋｍ／ｈ）が可能となる。
【図面の簡単な説明】
【００２５】
【図１】特許文献１〜３の手段が適用される車両の説明図である。
【図２】一対の撮像手段間での２次元射影変換を説明するための模式図である。
【図３】特許文献１〜３による平面領域を抽出する手順の説明図である。
【図４】特許文献４による自律移動車両の側面図である。
【図５】本発明が適用される自律移動ロボットの説明図である。
【図６】自律移動ロボットのブロック図である。
【図７】自律移動ロボットの使用状態を示す側面図である。
【図８】自律移動ロボットの特殊な使用状態を示す側面図である。
【図９】移動ロボット１０の特殊な使用状態を示す別の説明図である。
【図１０】自己位置評定用コンピュータ、画像処理用コンピュータ、及び環境地図生成用コンピュータの動作フロー図である。
【図１１】車両制御用コンピュータの動作フロー図である。
【図１２】レーザセンサによる障害物検出フロー図である。
【図１３】図１２の説明図である。
【図１４】本発明の遠方環境認識装置から出力する環境地図データの説明図である。
【発明を実施するための形態】
【００２６】
以下、本発明の好ましい実施形態を図面を参照して説明する。なお各図において、共通する部分には同一の符号を付し、重複した説明は省略する。
【００２７】
図５は、本発明が適用される自律移動ロボットの説明図であり、図６は自律移動ロボットのブロック図である。
【００２８】
図５、図６において、自律移動ロボット１０は、ＧＰＳ１１、ジャイロセンサ１２、自己位置評定用コンピュータ１３、車両制御装置１４及び操縦装置１５を備える。
自律移動ロボット１０は、この例では自走する車両であるが、本発明はこれに限定されず、半自律移動ロボットや操縦支援装置であってもよい。なお、以下の説明では、自律移動ロボット１０を、単に「移動ロボット」又は「車両」と呼ぶ。
【００２９】
車両制御装置１４は、この例では経路生成用コンピュータ１４ａ、車両制御用コンピュータ１４ｂ、ドライバ１４ｃ及びモータ、アクチュエータ１４ｄからなり、経路生成用コンピュータ１４ａにより自律的に走行経路の生成を行い、車両制御用コンピュータ１４ｂ、ドライバ１４ｃ及びモータ、アクチュエータ１４ｄにより車両を操縦して走行するようになっている。
操縦装置１５は、この例ではノートパソコンであり、人がノートパソコン１５によりＧＰＳ座標系でウェイポイントを入力し、経路生成用コンピュータ１４ａに無線で通信するようになっている。
またＧＰＳ１１、ジャイロセンサ１２及び自己位置評定用コンピュータ１３により周辺の地図、車両の位置、車両の姿勢を検出し、経路生成用コンピュータ１４ａに出力するようになっている
【００３０】
上述した構成により、人がＧＰＳ座標系でウェイポイントを入力することにより、経路生成用コンピュータ１４ａによりウェイポイントを通過するように移動ロボット１０は自律的に走行路検出及び経路生成を行い車両を操縦して走行することができる。
【００３１】
本発明の遠方環境認識装置２０は、上述した移動ロボット１０に走行経路の生成に必要な環境地図データ５を出力する装置である。
【００３２】
図５、図６において、本発明の遠方環境認識装置２０は、一対の車載カメラ２２、レーザセンサ２４、及びコンピュータ２６を備える。
コンピュータ２６は、この例では画像処理用コンピュータ２６ａと環境地図生成用コンピュータ２６ｂからなり、車載カメラ２２とレーザセンサ２４の出力から、単一の座標系に定義された平面領域及び障害物を含む環境地図データを生成する。
【００３３】
図７は、移動ロボット１０の使用状態を示す側面図である。
この図において、一対の車載カメラ２２は、例えばステレオカメラであり、移動ロボット１０に固定して配置され、前方の平面領域１を含む相互に重複する領域の画像２（ステレオ画像）を撮像する。
画像処理用コンピュータ２６ａは、上述した特許文献１〜３の手段により、車載カメラ２２からのステレオ画像２を利用して、平面領域１と、平面領域１の法線ベクトルｎ（カメラ座標系における法線ベクトル）を検出する。
【００３４】
レーザセンサ２４は、この例では、移動ロボット１０に固定して配置されたレーザセンサである。またこのレーザセンサ２４は、４本のスキャンラインを有しており、前方正面及び所定の検出必要距離に相当する下向き角度でレーザ光３ａ〜３ｄを前方左右に走査しその反射光から障害物４の位置を検出するようになっている。
【００３５】
この例で、レーザ光３ａは、前方正面に水平に照射され、その反射光から、前方に位置する障害物４の位置（方向及び距離）を検出する。また、レーザ光３ｂ〜３ｄは、レーザ光３ａから一定の角度（例えば、０．５〜１．０度）ずつ下向きに傾斜しており、それぞれの反射光から、前方に位置する障害物４の位置（方向及び距離）を検出するようになっている。
【００３６】
また、本発明の方法では、路面と正対物の再帰反射率の違いに着眼し、レーザセンサ２４の反射光の受光強度により障害物の有無を検出する。
すなわち、画像処理用コンピュータ２６ａによる平面検出により検出することができなかった、路面上の小さな障害物をレーザセンサ２４の反射光の受光強度により検出する。
レーザセンサ２４のレーザ光３ｂ〜３ｄは平面領域１に対しては浅い入射角となり、受光強度は弱い。一方、正対する障害物４は深い入射角となり高い受光強度が得られる。本発明の方法では、この差により、障害物４の検出を行う。また、計測が遠距離となるため、レーザセンサから算出される障害物位置にも誤差があるので、これを考慮した処理を行う。
すなわち、遠方の計測であることから、障害物位置に対応する地図セルに投票して、地図上を探索窓によって探索し、探索窓内の投票値が閾値を超える場合に、その窓内のセルを障害物として、レーザセンサから算出される障害物位置の計測誤差を補正する。
【００３７】
環境地図生成用コンピュータ２６ｂは、自己位置評定用コンピュータ１３、車載カメラ２２及びレーザセンサ２４の出力、すなわち、周辺の地図、車両の位置、車両の姿勢、平面領域１、平面領域１の法線ベクトルｎ、及び障害物４の位置から、単一の座標系に定義された平面領域１及び障害物４を含む環境地図データ５を生成する。
この環境地図データ５は、車両制御装置１４の経路生成用コンピュータ１４ａに入力され、経路生成用コンピュータ１４ａにより、自律的に走行路検出及び経路生成を行うために用いられる。
【００３８】
図８は、移動ロボット１０の特殊な使用状態を示す側面図である。
この例では、移動ロボット１０が走行中の路面１ａに対して前方の走行可能領域１が傾いており、検出必要距離内に存在する障害物４を検出できない場合を示している。
すなわち現在走行する路面１ａに対して前方の走行可能領域１が上述したレーザ光３ａ〜３ｄの下向き角度（例えば最大約３度）以上に傾くと、レーザ光３ａ〜３ｄが路面（走行可能領域１）から外れてしまう。
レーザセンサ２４が反射光を検出せず、レーザセンサ２４から測距データが出力されないということは、障害物４が存在しないことを意味する。そのためレーザセンサ２４の走査範囲が、路面から外れる場合は、障害物４は検出されず、このため、障害物発見の遅れとなる問題が発生する。
この問題を回避するために、本発明では、新たに検出された前方の平面領域１のカメラ座標系における法線ベクトルｎが、カメラ座標系において所定の範囲を超える場合に、レーザセンサの走査領域が路面上から外れ、障害物検出が有効ではないとして、新たに検出された前方の平面領域を除外して前記環境地図データ５を生成する。
すなわち、本発明では、移動ロボット１０から見た平面領域１の姿勢角とレーザセンサ２４のレーザ光３ａ〜３ｄの成す角が、想定範囲から外れた場合には、平面検出処理によって新たに検出された前方の平面領域を地図に反映させないという処理をとることで、上記問題を回避する。
この場合、例えば車両１０は元の平面領域内で減速し、前記角度が閾値内に戻りしだい、新たに検出された前方の平面領域を地図に反映させて、高速走行を続行する。
【００３９】
図９は、移動ロボット１０の特殊な使用状態を示す別の説明図である。この図において、（Ａ）は走行領域に対する車両１０の角度が正常な場合、（Ｂ）は車両１０にピッチ角が発生した場合、（Ｃ）は車両１０にロール角が発生した場合である。
図９（Ｂ）の車両１０にピッチ角が発生した場合は、図８の場合に相当する。この場合、レーザ光のスキャン範囲２７の全体において、レーザセンサ２４が反射光を検出せず、レーザ光のスキャンが有効ではない（無効）ため、平面検出結果を反映させない。
【００４０】
一方、図９（Ｃ）の車両１０にロール角が発生した場合は、レーザ光のスキャン範囲２７の一部でレーザが有効であるため、この有効範囲では平面検出結果を反映する。
すなわち車両１０が例えば時計方向にロールしていて、前方は水平な平面であると画像で検出できた場合、レーザセンサ２４ではある角度以上に左側に向いた方向が所定の角度以上に上空を向いてしまい、無効な計測となる。このような場合を想定して、レーザ光のスキャン範囲２７の全部の有効・無効を判断するのではなく、スキャンのうち、有効な扇状のエリアを、計測点の向きと平面の向きから決定する。
この場合、新たに検出された前方の平面領域のカメラ座標系における法線ベクトルから、走行領域に対する車両の角度を算出し、この角度から、レーザ光が地面上の障害物を検出しうる範囲にある領域を算出し、この領域と重複する平面検出領域を選択して、前記環境地図データに反映する。
【００４１】
図１０は、自己位置評定用コンピュータ、画像処理用コンピュータ、及び環境地図生成用コンピュータの動作フロー図であり、Ｓ１〜Ｓ４の各ステップ（工程）を有する。
ノートパソコン１５により、ＧＰＳ座標系でウェイポイントを入力し、車両制御用コンピュータ１４に無線で入力する。
自己位置評定用コンピュータ１３は、ＧＰＳ１１とジャイロセンサ１２からデータを受信し（Ｓ１）、周辺の地図、車両の位置、車両の姿勢を検出し、車両制御用コンピュータ１４に出力する。
画像処理用コンピュータ２６ａは、車載カメラ２２からのステレオ画像２を利用して、平面領域１の地図と、車両から見た平面姿勢角（すなわち平面領域１の法線ベクトルｎ）を検出し（Ｓ２）、環境地図生成用コンピュータ２６ｂと車両制御用コンピュータ１４に出力する。
環境地図生成用コンピュータ２６ｂは、自己位置評定用コンピュータ１３の出力（すなわち、周辺の地図、車両の位置、車両の姿勢）とレーザセンサ２４の出力から、障害物の検出処理を行い（Ｓ３）、障害物マップ４ａを出力する。さらにこの環境地図生成用コンピュータ２６ｂは、障害物マップ４ａと車載カメラ２２の出力（すなわち、平面領域１、平面領域１の法線ベクトルｎ）から、地図データを合成して、単一の座標系に定義された平面領域１及び障害物４を含む環境地図データ５を生成し（Ｓ４）、車両制御用コンピュータ１４に出力する。
【００４２】
図１１は、車両制御用コンピュータの動作フロー図であり、Ｓ１１〜Ｓ１８の各ステップ（工程）を有する。
車両制御用コンピュータ１４は、ノートパソコン１５からウェイポイントを受信し、自己位置評定用コンピュータ１３から車両位置と車両姿勢を受信し、環境地図生成用コンピュータ２６ｂから環境地図データ５を受信し、画像処理用コンピュータ２６ａから車両から見た平面姿勢角を受信する。
車両制御用コンピュータ１４は、ウェイポイント、車両位置、車両姿勢及び環境地図データ５から、ポテンシャル法によりポテンシャル場を生成し（Ｓ１１）、最急降下法により経路を設定する（Ｓ１２）。このポテンシャル法と最急降下法は、車両位置や障害物位置のポテンシャルを高く設定し、ウェイポイントや平面領域のポテンシャルを低く設定して、ポテンシャルの高い方から低い方に向かって水が流れるような方向に経路を設定する手法である。
【００４３】
車両制御用コンピュータ１４は、次いで、旋回半径から車速目標値Ａを設定し（Ｓ１３）、さらに平面姿勢角と車両姿勢から車速目標値Ｂを設定し（Ｓ１４）、車速目標値Ａ，Ｂの小さい方を採用する（Ｓ１５）。
次いで、車両制御用コンピュータ１４は、アクセル、ブレーキ位置の目標値を設定し（Ｓ１６）、旋回半径からステアリング位相の目標値を設定し（Ｓ１７）、モータ、アクチュエータを作動させる（Ｓ１８）。
以上の各ステップを車両制御用コンピュータ１４は、所定の制御間隔Δｔで繰り返し実施し、自律的に走行経路の生成を行い、車両を操縦して走行する。
【００４４】
図１２は、レーザセンサによる障害物検出フロー図であり、Ｓ２１〜Ｓ３０の各ステップ（工程）を有する。また図１３は図１２の説明図である。
図１２において、レーザセンサ２４は、前方正面及び所定の検出必要距離に相当する下向き角度でレーザ光３ａ〜３ｄを前方左右に走査し、その反射光の受光強度が所定の閾値以上であるものから測距値を算出する。この場合、車両の走行に支障がある大きさの障害物を確実に検出するように、レーザ光３ａ〜３ｄの走査密度（すなわち走査レート）を設定する。
検出されたＮ０個のデータ（位相と測距値）を出力し（Ｓ２１）、そのデータから環境認識に必要な位相範囲内のデータＮ個を抽出し（Ｓ２２）、車両姿勢角と自己位置を用いて座標変換によりＮ個の慣性座標特徴（Ｘｅ，Ｙｅ，Ｚｅ）を算出する（Ｓ２３）。この場合、測距データがない場所は、障害物がないとして扱う。
図１３（Ａ）はこの時点（Ｓ２３）での障害物候補マップを示している。この図において、ａは車両位置、ｂは環境認識に必要な位相範囲、ｃは障害物候補である。
【００４５】
次いで、図１２において、障害物候補マップを呼び出し投票作業へ移行し（Ｓ２４）、Ｎ個の障害物候補に対して、座標（Ｘｅ，Ｙｅ）を地図分解能δで除し、投票セル位置（ｉ，ｊ）を算出し（Ｓ２５）、投票セル位置（ｉ，ｊ）に投票する（Ｓ２６）。
次にｉ＝ｉｓ〜ｉｅ、ｊ＝ｊｓ〜ｊｅの環境認識範囲において、探索窓内の投票数の総和を算出し（Ｓ２７）、投票数の総和が所定の閾値以上である場合（Ｓ２８）に、探索窓内の地図セルを障害物に設定する（Ｓ２９）。
図１３（Ｂ）は、探索窓ｄ内の投票数の総和を算出する状態を示している。
【００４６】
次いで、図１２において、障害物候補マップから、環境認識範囲で障害物情報を切り出し、障害物マップとして出力する（Ｓ３０）。図１３（Ｃ）は、障害物マップの例であり、この図でｅは環境認識に必要な範囲、ｆは障害物である。
【００４７】
図１４は、本発明の遠方環境認識装置２０から出力する環境地図データの説明図である。この図において、（Ａ）はある時刻ｔまでに検出した平面領域１ａ、（Ｂ）は時刻ｔに検出した平面領域１、（Ｃ）は時刻ｔに検出した障害物領域ｆを示している。
【００４８】
図１４（Ａ）に示す時刻ｔまでに検出した平面領域１ａは、走行可能領域であり、車両ａは安全に走行できる。
図１４（Ｂ）に示す時刻ｔに検出した平面領域１は、画像処理用コンピュータ２６ａにより車載カメラ２２からのステレオ画像２を利用して検出された平面領域１であり、障害物がない限り車両ａは安全に走行できる。
図１４（Ｃ）に示す時刻ｔに検出した障害物領域ｆは、レーザセンサ２４により上述した方法で検出される。
図１４（Ｃ）の図は、上述した単一の座標系に定義された平面領域１及び障害物４を含む環境地図データ５に相当しており、この環境地図データ５に基づき、経路生成用コンピュータ１４ａにより、自律的に走行路検出及び経路生成を行うことができる。
【００４９】
また、図８に示したように、レーザセンサ２４の走査範囲が、路面から外れる場合は、障害物４は検出されず、このため、障害物発見の遅れとなるが、本発明では、移動ロボット１０から見た平面領域１の姿勢角とレーザセンサ２４のレーザ光３ａ〜３ｄの成す角が、想定範囲から外れた場合には、平面検出処理によって検出された図１４（Ｂ）の平面領域１を地図に反映させないという処理をとることで、図１４（Ａ）に示す平面領域１ａのみを走行可能領域とするので、車両ａは安全に走行することができる。
【００５０】
上述した本発明の装置及び方法によれば、一対の車載カメラ２２、レーザセンサ２４、及びコンピュータ２６ａ，２６ｂからなる小型かつ安価な装置により、単一の座標系に定義された平面領域１及び障害物４を含む環境地図データ５を生成するので、この環境地図データ５を用いて、自律移動ロボット１０が自律的に走行経路の生成を行って走行することができる。
【００５１】
また、レーザセンサ２４が前方正面及び所定の検出必要距離に相当する下向き角度でレーザ光３ａ〜３ｄを前方左右に走査し、その反射光から障害物４の位置を検出するので、高速走行時に検出が必要な検出必要距離内に存在する小さな障害物４を検出することができる。
【００５２】
また、新たに検出された前方の平面領域１の法線ベクトルｎと、走行中の路面１ａの法線ベクトルｎａとの角度が、所定の閾値を超える場合に、新たに検出された前方の平面領域１を除外して環境地図データ５を生成するので、走行中の路面１ａに対して前方の走行可能領域１が傾いており、検出必要距離内に存在する障害物４を検出できない場合でも、元の走行可能領域のみが表示されるので、走行安全性を確保することができる
【００５３】
従って、ステレオカメラ（一対の車載カメラ）、レーザセンサの組合せにより、相互の欠点を補うことができ、遠方の環境認識が実現できる。
また小さな障害物が存在する場合でも、遠方（現状約４０ｍ）の環境認識が実現できるため、高速走行（現状約５０ｋｍ／ｈ）が可能となる。
【００５４】
なお、本発明は上述した実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々に変更することができることは勿論である。
【符号の説明】
【００５５】
１ 平面領域、１ａ 走行中の路面、２ 画像（ステレオ画像）、
３ａ〜３ｄ レーザ光、４ 障害物、５ 環境地図データ、
１０ 自律移動ロボット（移動ロボット、車両）、１１ ＧＰＳ、
１２ ジャイロセンサ、１３ 自己位置評定用コンピュータ、
１４ 車両制御装置、１４ａ 経路生成用コンピュータ、
１４ｂ 車両制御用コンピュータ、１４ｃドライバ、
１４ｄ モータ、アクチュエータ、１５ 操縦装置、
２０ 遠方環境認識装置、２２ 車載カメラ（ステレオカメラ）、
２４ レーザセンサ、
２６ コンピュータ、２６ａ 画像処理用コンピュータ、
２６ｂ 環境地図生成用コンピュータ、２７ スキャン範囲
【特許請求の範囲】
【請求項１】
移動ロボットに、走行経路の生成に必要な環境地図データを出力する遠方環境認識装置であって、
前記移動ロボットに固定して配置され、前方の平面領域を含む相互に重複する領域の画像を撮像する一対の車載カメラと、
前記移動ロボットに固定して配置され、レーザ光を前方左右に走査しその反射光から障害物の位置を検出するレーザセンサと、
前記車載カメラとレーザセンサの出力から、単一の座標系に定義された平面領域及び障害物を含む前記環境地図データを生成するコンピュータと、を備えたことを特徴とする遠方環境認識装置。
【請求項２】
前記移動ロボットは、自律的に走行経路の生成を行って走行する自律移動ロボット、半自律移動ロボット、又は遠隔で操縦する操縦支援装置である、ことを特徴とする請求項１に記載の遠方環境認識装置。
【請求項３】
前記レーザセンサは、複数のスキャンラインを持つレーザセンサである、ことを特徴とする請求項１に記載の遠方環境認識装置。
【請求項４】
移動ロボットに、走行経路の生成に必要な環境地図データを出力する遠方環境認識方法であって、
前記移動ロボットに固定して配置された一対の車載カメラにより、前方の平面領域を含む相互に重複する領域の画像を撮像し、
前記移動ロボットに固定して配置されたレーザセンサにより、レーザ光を前方左右に走査しその反射光から障害物の位置を検出し、
前記画像から前記平面領域と該平面領域のカメラ座標系における法線ベクトルとを検出し、
前記平面領域、平面領域の法線ベクトル、障害物の位置から単一の座標系に定義された平面領域及び障害物を含む前記環境地図データを生成する、ことを特徴とする遠方環境認識方法。
【請求項５】
路面と正対物の再帰反射率の違いに着眼し、前記レーザセンサの反射光の受光強度により障害物の有無を検出する、ことを特徴とする請求項４に記載の遠方環境認識方法。
【請求項６】
障害物位置に対応する地図セルに投票して、地図上を探索窓によって探索し、探索窓内の投票値が閾値を超える場合に、その窓内のセルを障害物として、前記レーザセンサから算出される障害物位置の計測誤差を補正する、ことを特徴とする請求項４に記載の遠方環境認識方法。
【請求項７】
新たに検出された前方の平面領域のカメラ座標系における法線ベクトルが、カメラ座標系において所定の範囲を超える場合に、新たに検出された前方の平面領域を除外して前記環境地図データを生成する、ことを特徴とする請求項４に記載の遠方環境認識方法。
【請求項８】
新たに検出された前方の平面領域のカメラ座標系における法線ベクトルから、走行領域に対する車両の角度を算出し、この角度から、レーザ光が地面上の障害物を検出しうる範囲にある領域を算出し、この領域と重複する平面検出領域を選択して、前記環境地図データに反映する、ことを特徴とする請求項４に記載の遠方環境認識方法。
【請求項１】
移動ロボットに、走行経路の生成に必要な環境地図データを出力する遠方環境認識装置であって、
前記移動ロボットに固定して配置され、前方の平面領域を含む相互に重複する領域の画像を撮像する一対の車載カメラと、
前記移動ロボットに固定して配置され、レーザ光を前方左右に走査しその反射光から障害物の位置を検出するレーザセンサと、
前記車載カメラとレーザセンサの出力から、単一の座標系に定義された平面領域及び障害物を含む前記環境地図データを生成するコンピュータと、を備えたことを特徴とする遠方環境認識装置。
【請求項２】
前記移動ロボットは、自律的に走行経路の生成を行って走行する自律移動ロボット、半自律移動ロボット、又は遠隔で操縦する操縦支援装置である、ことを特徴とする請求項１に記載の遠方環境認識装置。
【請求項３】
前記レーザセンサは、複数のスキャンラインを持つレーザセンサである、ことを特徴とする請求項１に記載の遠方環境認識装置。
【請求項４】
移動ロボットに、走行経路の生成に必要な環境地図データを出力する遠方環境認識方法であって、
前記移動ロボットに固定して配置された一対の車載カメラにより、前方の平面領域を含む相互に重複する領域の画像を撮像し、
前記移動ロボットに固定して配置されたレーザセンサにより、レーザ光を前方左右に走査しその反射光から障害物の位置を検出し、
前記画像から前記平面領域と該平面領域のカメラ座標系における法線ベクトルとを検出し、
前記平面領域、平面領域の法線ベクトル、障害物の位置から単一の座標系に定義された平面領域及び障害物を含む前記環境地図データを生成する、ことを特徴とする遠方環境認識方法。
【請求項５】
路面と正対物の再帰反射率の違いに着眼し、前記レーザセンサの反射光の受光強度により障害物の有無を検出する、ことを特徴とする請求項４に記載の遠方環境認識方法。
【請求項６】
障害物位置に対応する地図セルに投票して、地図上を探索窓によって探索し、探索窓内の投票値が閾値を超える場合に、その窓内のセルを障害物として、前記レーザセンサから算出される障害物位置の計測誤差を補正する、ことを特徴とする請求項４に記載の遠方環境認識方法。
【請求項７】
新たに検出された前方の平面領域のカメラ座標系における法線ベクトルが、カメラ座標系において所定の範囲を超える場合に、新たに検出された前方の平面領域を除外して前記環境地図データを生成する、ことを特徴とする請求項４に記載の遠方環境認識方法。
【請求項８】
新たに検出された前方の平面領域のカメラ座標系における法線ベクトルから、走行領域に対する車両の角度を算出し、この角度から、レーザ光が地面上の障害物を検出しうる範囲にある領域を算出し、この領域と重複する平面検出領域を選択して、前記環境地図データに反映する、ことを特徴とする請求項４に記載の遠方環境認識方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【公開番号】特開２０１１−１３４２２６（Ｐ２０１１−１３４２２６Ａ）
【公開日】平成２３年７月７日（２０１１．７．７）
【国際特許分類】
【出願番号】特願２００９−２９４８１５（Ｐ２００９−２９４８１５）
【出願日】平成２１年１２月２５日（２００９．１２．２５）
【出願人】（５００３０２５５２）株式会社ＩＨＩエアロスペース (298)
【出願人】（０００００００９９）株式会社ＩＨＩ (5,014)
【Ｆターム（参考）】
【公開日】平成２３年７月７日（２０１１．７．７）
【国際特許分類】
【出願日】平成２１年１２月２５日（２００９．１２．２５）
【出願人】（５００３０２５５２）株式会社ＩＨＩエアロスペース (298)
【出願人】（０００００００９９）株式会社ＩＨＩ (5,014)
【Ｆターム（参考）】
[ Back to top ]