移動経路計画装置、移動体制御装置及び移動体
【課題】移動体の経路計画効率の向上を図ることが可能な移動経路計画装置を提供する。
【解決手段】移動体の進行方向を撮影する撮影手段と、前記撮影手段から得られる撮影画像を基に所定の画像処理を行うことにより、前記撮影画像に存在する通行痕跡を検出する画像処理手段と、前記画像処理手段によって検出された通行痕跡を基に前記移動体の移動経路を計画する経路計画手段とを備える。
【解決手段】移動体の進行方向を撮影する撮影手段と、前記撮影手段から得られる撮影画像を基に所定の画像処理を行うことにより、前記撮影画像に存在する通行痕跡を検出する画像処理手段と、前記画像処理手段によって検出された通行痕跡を基に前記移動体の移動経路を計画する経路計画手段とを備える。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、移動経路計画装置、移動体制御装置及び移動体に関する。
【背景技術】
【0002】
従来では、不整地における移動体(例えば車両)の制御手法として、以下の手法が採用されていた。まず、ステレオ画像計測やレーザレンジファインダ(LRF)等を用いて周囲の3次元形状を計測し、その計測結果に基づいて、大きな突起物等を障害物として検出する。そして、障害物から接触を避けられるだけの十分な距離を確保しつつ、且つ、経路上の起伏やカーブ等の減速要因・動揺など、運行を不安定にする要因が最小となるように車両の経路や速度等を計画し、その計画に従って車両の加減速、ステアリング角を求め、各アクチュエータによって車両のステアリング及び車輪の駆動を行う。
【0003】
なお、従来の移動体制御手法として、下記特許文献1には、周囲の3次元形状を計測して行動可能範囲(環境地図)を判断し、その行動可能範囲内で経路計画を行う手法が開示されている。また、下記特許文献2には、地形センサによるセンシングで経路探索地図を作成し、複数の経路候補の中から最も平坦な経路を走行上の最適経路として選択する手法が開示されている。また、下記特許文献3には、障害物の検出から各アクチュエータの制御に至るまでの自律走行制御システムの具体的な構成例が開示されている。さらに、下記特許文献4には、将来見通せる距離が短い場合の対処の例として、逐次再計画する手法が開示されている。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特開2005−092820号公報
【特許文献2】特開平07−064631号公報
【特許文献3】特開平11−212640号公報
【特許文献4】特開平11−007318号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
例えば、上述したステレオ画像計測やLRF等を用いて、図19(a)や(b)に示すような不整地の3次元形状を計測し、その計測結果に基づいて車両の経路計画を行う場合を想定する。この場合、図19(a)や(b)のような画像を撮影した場所において、周囲の3次元形状が正確に得られる近傍領域(ステレオ画像計測で十分な精度が出る範囲、またはLRFで計測している範囲で画像中の真ん中より下に相当する領域)は広い範囲で平坦であり、どのような経路もとりうるため、目標地点に向けて直行するような経路(図中の実線の矢印)を計画しがちである。
【0006】
ところが、そのように計画した経路に沿って車両を走行させ、ある一定以上の遠方まで進んだ地点で障害物が存在して行き止まりになり、急に前方が通行不能であることが判明した場合、障害物を回避して新たに経路を計画し直す必要が生じる。このように、従来の経路計画手法では、障害物を回避可能な理想的な経路(図中の破線の矢印)と、計画された経路とが必ずしも一致せず、経路計画効率の低下を招くという問題があった。
【0007】
本発明は、上述した事情に鑑みてなされたものであり、経路計画効率の向上を図ることが可能な移動経路計画装置、移動体制御装置及び移動体を提供することを目的とする。
【課題を解決するための手段】
【0008】
上記課題を解決するために、本発明では、移動経路計画装置に係る第1の解決手段として、移動体の進行方向を撮影する撮影手段と、前記撮影手段から得られる撮影画像を基に所定の画像処理を行うことにより、前記撮影画像に存在する通行痕跡を検出する画像処理手段と、前記画像処理手段によって検出された通行痕跡を基に前記移動体の移動経路を計画する経路計画手段とを備えることを特徴とする。
【0009】
また、移動経路計画装置に係る第2の解決手段として、上記第1の解決手段において、移動体の進行方向の3次元形状を計測する3次元計測手段と、前記3次元計測手段から得られる3次元形状の計測結果を基に地図を作成する地図作成手段と、をさらに備え、前記経路計画手段は、前記地図作成手段によって作成された地図及び前記画像処理手段によって検出された通行痕跡を基に前記移動体の移動経路を計画することを特徴とする。
【0010】
また、移動経路計画装置に係る第3の解決手段として、上記第1または第2の解決手段において、前記画像処理手段は、幅を有する帯状の模様を前記通行痕跡として検出することを特徴とする。
【0011】
また、移動経路計画装置に係る第4の解決手段として、上記第1または第2の解決手段において、前記画像処理手段は、幅を有し、途中で断続的になっている帯状の模様が複数、3次元空間上で平行になっている部分を前記通行痕跡として検出することを特徴とする。
【0012】
また、移動経路計画装置に係る第5の解決手段として、上記第1または第2の解決手段において、前記画像処理手段は、幅を有し、途中で断続的になっている帯状の部分を前記撮影画像中から見つけ出し、最も多数の帯状部分が共有する消失点に向かっている帯状部分を前記通行痕跡として絞り込むことを特徴とする。
【0013】
また、移動経路計画装置に係る第6の解決手段として、上記第1または第2の解決手段において、前記画像処理手段は、方向パラメータの異なる複数の空間周波数フィルタを用いて、前記撮影画像における検出対象領域をフィルタリングすることにより、前記検出対象領域についての各方向の強度分布データを生成するフィルタリング処理と、前記フィルタリング処理によって生成された前記強度分布データを基に前記検出対象領域における各画素の模様方向を特定する模様方向特定処理と、前記模様方向特定処理による前記模様方向の特定結果を基に所定の統計処理を行うことにより、前記検出対象領域において通行痕跡が存在すると推定される痕跡領域を検出する痕跡領域検出処理とを行うことを特徴とする。
【0014】
また、移動経路計画装置に係る第7の解決手段として、上記第1または第2の解決手段において、前記画像処理手段は、方向パラメータの異なる複数の空間周波数フィルタを用いて、前記撮影画像における検出対象領域をフィルタリングすることにより、前記検出対象領域についての各方向の強度分布データを生成するフィルタリング処理と、前記フィルタリング処理によって生成された前記強度分布データを基に前記検出対象領域における各画素の模様方向を特定する模様方向特定処理と、前記検出対象領域を奥行き方向に沿って複数に分割して得られる副検出対象領域ごとに、前記模様方向特定処理による前記模様方向の特定結果に基づいて所定の統計処理を行うことにより、前記副検出対象領域において通行痕跡が存在すると推定される痕跡領域を検出する痕跡領域検出処理とを行うことを特徴とする。
【0015】
また、移動経路計画装置に係る第8の解決手段として、上記第6の解決手段において、前記画像処理手段は、前記痕跡領域検出処理における前記統計処理として、前記検出対象領域における各画素のそれぞれを投票元画素として選択し、前記投票元画素を起点としてその模様方向に平行な直線上に存在する画素、または前記投票元画素を起点としてその模様方向に沿うように設定された投票対象領域に含まれる画素に対して投票を行う投票処理と、前記投票処理による投票結果を基に投票回数が最も大きい画素を消失点として特定する消失点特定処理と、前記消失点の投票元画素を有効投票画素として抽出する有効投票画素抽出処理と、前記検出対象領域において前記消失点を頂点とする領域を複数の小領域に分割し、各小領域の内、前記有効投票画素の比率が所定の閾値を超えた小領域を前記痕跡領域として特定する痕跡領域特定処理とを行うことを特徴とする。
【0016】
また、移動経路計画装置に係る第9の解決手段として、上記第7の解決手段において、前記画像処理手段は、前記痕跡領域検出処理における前記統計処理として、前記副検出対象領域における各画素のそれぞれを投票元画素として選択し、前記投票元画素を起点としてその模様方向に平行な直線上に存在する画素、または前記投票元画素を起点としてその模様方向に沿うように設定された投票対象領域に含まれる画素に対して投票を行う投票処理と、前記投票処理による投票結果を基に投票回数が最も大きい画素を消失点として特定する消失点特定処理と、前記消失点の投票元画素を有効投票画素として抽出する有効投票画素抽出処理と、前記副検出対象領域において前記消失点を頂点とする領域を複数の小領域に分割し、各小領域の内、前記有効投票画素の比率が所定の閾値を超えた小領域を前記痕跡領域として特定する痕跡領域特定処理とを行うことを特徴とする。
は、2次元ガボールフィルタであることを特徴とする。
【0017】
また、移動経路計画装置に係る第10の解決手段として、上記第9の解決手段において、前記画像処理手段は、前記投票処理では、現在着目している副検出対象領域において、以前着目していた副検出対象領域について実施された前記痕跡領域特定処理によって特定された痕跡領域の終端側に、投票の重み付けを大きくした投票重み付け領域を設定することを特徴とする。
【0018】
また、移動経路計画装置に係る第11の解決手段として、上記第10の解決手段において、前記画像処理手段は、現在着目している副検出対象領域について前記痕跡領域特定処理が終了した後に、以前着目していた副検出対象領域において、前記現在着目している副検出対象領域について実施された前記痕跡領域特定処理によって特定された痕跡領域の始端側に、投票の重み付けを大きくした投票重み付け領域を設定し、当該以前着目していた副検出対象領域について前記投票処理、消失点特定処理、有効投票画素抽出処理及び痕跡領域特定処理を再実施することで、当該以前着目していた副検出対象領域についての痕跡領域特定結果を修正することを特徴とする。
【0019】
また、移動経路計画装置に係る第12の解決手段として、上記第10または第11の解決手段において、前記画像処理手段は、前記奥行き方向に沿った位置に応じて前記投票重み付け領域の大きさ、及び/または前記投票の重み付け量を変化させることを特徴とする。
【0020】
一方、本発明では、移動体制御装置に係る第1の解決手段として、上記の第1〜第12の解決手段のいずれかを有する移動経路計画装置と、移動体の動力を発生するアクチュエータと、前記移動経路計画装置によって計画された移動経路に沿って移動体が移動するように前記アクチュエータを制御する運動制御手段とを備えることを特徴とする。
【0021】
また、移動体制御装置に係る第2の解決手段として、上記第1の解決手段において、前記移動体の位置に関する情報を取得する位置情報取得手段と、前記移動体の姿勢に関する情報を取得する姿勢情報取得手段と、前記位置情報取得手段及び前記姿勢情報取得手段によって取得された情報を基に前記移動体の現在位置及び姿勢を推定する位置・姿勢推定手段を備え、前記運動制御手段は、前記位置・姿勢推定手段によって推定された移動体の現在位置及び姿勢を基に前記移動体が前記移動経路を中心とする所定の許容範囲内を移動するように前記アクチュエータを制御することを特徴とする。
さらに、本発明では、移動体に係る解決手段として、上記の第1または第2の解決手段を有する移動体制御装置を搭載することを特徴とする。
【発明の効果】
【0022】
本発明によれば、不整地のように全体的に平坦・なだらかな地形であっても、そこある道のような通行痕跡(轍、ふみ跡など)を検出することが可能である。そして、不整地における通行痕跡は、実際に以前から何度か車両が通行していた結果生じるものであり、その延長線上は継続して通行可能なエリアになっている可能性が極めて高い。このような通行痕跡を使って計測エリア外の通行可能性が高い方向を予測することで、計測エリア外も含めて効率的な移動経路を計画することが可能となる。
【図面の簡単な説明】
【0023】
【図1】本実施形態に係る移動体制御装置を搭載した車両Cの構成概略図である。
【図2】本実施形態に係る移動体制御装置の画像処理装置2のブロック構成図である。
【図3】本実施形態における入力画像及びその入力画像データの一例である。
【図4】本実施形態に係る画像処理装置2における信号処理装置2cの動作フローチャートである。
【図5】本実施形態に係る信号処理装置2cのフィルタリング処理に関する説明図である。
【図6】本実施形態に係る信号処理装置2cの模様方向特定処理に関する説明図である。
【図7】本実施形態に係る信号処理装置2cの痕跡領域検出処理における投票処理に関する第1説明図である。
【図8】本実施形態に係る信号処理装置2cの痕跡領域検出処理における投票処理に関する第2説明図である。
【図9】本実施形態に係る信号処理装置2cの痕跡領域検出処理における投票処理に関する第3説明図である。
【図10】本実施形態に係る信号処理装置2cの痕跡領域検出処理における消失点特定処理に関する説明図である。
【図11】本実施形態に係る信号処理装置2cの痕跡領域検出処理における有効投票画素抽出処理に関する説明図である。
【図12】本実施形態に係る信号処理装置2cの痕跡領域検出処理における痕跡領域特定処理に関する説明図である。
【図13】本実施形態に係る信号処理装置2cの痕跡領域検出処理の変形例に関する第1説明図である。
【図14】本実施形態に係る信号処理装置2cの痕跡領域検出処理の変形例に関する第2説明図である。
【図15】本実施形態に係る信号処理装置2cの痕跡領域検出処理の変形例に関する第3説明図である。
【図16】本実施形態に係る地図作成装置4によって作成された地図(3次元マップ)の一例である。
【図17】本実施形態に係る経路計画装置5によって計画された移動経路の一例である。
【図18】本実施形態に係る運動制御装置9の制御によって移動する車両Cの移動範囲の一例である。
【図19】従来における移動経路計画手法の説明図である。
【発明を実施するための形態】
【0024】
以下、図面を参照して、本発明の一実施形態について説明する。
図1は、本実施形態に係る移動体制御装置を搭載した移動体の構成概略図である。本実施形態では、移動体として移動体制御装置の制御によって自律走行する車両Cを例示して説明する。なお、図1(a)は車両Cの側面図であり、図1(b)は車両Cの上面図である。また、以下では、図1に示すように、車両Cの進行方向をX軸、X軸と直交する車両Cの幅方向をY軸、XY平面(水平面)に垂直な方向(車両Cの高さ方向)をZ軸とするXYZ直交座標系を設定して説明する。
【0025】
この図1に示すように、車両Cに搭載された移動体制御装置は、撮影装置1a、1b、画像処理装置2、3次元計測装置3、地図作成装置4、経路計画装置5、GPS受信機6、ジャイロ7、位置・姿勢推定装置8、運動制御装置9及びアクチュエータ10から構成されている。なお、上記の撮影装置1a、1b、画像処理装置2、3次元計測装置3、地図作成装置4及び経路計画装置5は、本発明における移動経路計画装置を構成するものである。
【0026】
撮影装置1a、1b(撮影手段)は、例えば赤外線カメラであり、車両Cの上部左右に対となるように設置され、車両Cの進行方向を撮影して撮影画像を示す信号(画像信号)を画像処理装置2に出力する。図1に示すように、これら撮影装置1a、1bによって垂直方向(Z軸方向)の視野40°、水平方向(X軸方向)の視野60°をカバーすることが望ましい。また、上空の情報は不要であるため、車体が振動しても水平線が撮影画像の上端より飛び出ない程度に下向きに撮影装置1a、1bを設置することが望ましい(例えばY軸に対して下向きに10°傾けて設置し、水平線より下30°、上10°が視野に収まるように撮影を行う)。
【0027】
なお、図1では図示していないが、必要に応じて赤外線の投光器を車両Cの上部に設置しても良い。また、本実施形態では2つの撮影装置1a、1bを用いた場合を例示しているが、撮影装置の設置数はこれに限定されず、1つの撮影装置によって十分な撮影領域を確保可能な場合は1つだけ設置すれば良く、逆に1つでは十分な撮影領域を確保できない場合には、必要に応じて複数の撮影装置を設置しても良い。また、撮影装置1a、1bとして赤外線カメラ以外のカメラを用いても良い。
【0028】
画像処理装置2(画像処理手段)は、例えば画像処理専用のPC(Personal Computer)であり、撮影装置1a、1bから入力される画像信号(つまり撮影画像)を基に所定の画像処理を行うことにより、撮影画像に存在する通行痕跡(例えば轍など)を検出し、その通行痕跡検出結果を経路計画装置5に出力する。以下、図2を参照して、この画像処理装置2の内部構成について詳細に説明する。
【0029】
図2は、画像処理装置2のブロック構成図である。図2に示すように、画像処理装置2は、A/Dコンバータ2a、記憶装置2b、信号処理装置2c及びRAM(Random Access Memory)2dから構成されている。
【0030】
A/Dコンバータ2aは、撮影装置1a、1bから入力される画像信号をデジタルデータに変換し、入力画像(撮影画像)を示す入力画像データとして信号処理装置2cに出力する。例えば、撮影装置1a、1bの解像度をVGA(横640×縦480画素)と想定すると、上記の入力画像データは、入力画像を構成する横640×縦480個の各画素の輝度を示す輝度データの集合となる。なお、撮影装置1a、1bからデジタルデータ化された画像信号が出力される場合には、A/Dコンバータ2aを省略しても良い。
【0031】
記憶装置2bは、例えばHDD(Hard Disk Drive)であり、画像処理用プログラムPGと、方向パラメータの異なる複数(n個)の2次元ガボールフィルタ(以下、ガボールフィルタと略す)GF1〜GFnとを予め記憶しており、信号処理装置2cからの読み出し要求に応じて上記画像処理用プログラムPGやガボールフィルタGF1〜GFnを信号処理装置2cに出力する。
【0032】
ここで、画像処理用プログラムPGとは、入力画像データを基に入力画像に存在する通行痕跡を検出するための画像処理(フィルタリング処理、模様方向特定処理、強エッジ領域抽出処理、痕跡領域検出処理)を信号処理装置2cに実行させるためのプログラムである。
【0033】
また、ガボールフィルタとは、2次元ガウス関数と2次元平面上を一方向に伝播する正弦波関数とを乗じて得られる2次元ガボール関数を利用した空間周波数フィルタであり、2次元ウェーブレットフィルタの一形態である。横方向をX軸、縦方向をY軸とする2次元座標系を想定すると、2次元ガボール関数GB(x、y)は下記(1)式で表される。なお、下記(1)式において、u0は波の角周波数、σはガウス関数の標準偏差(ガウス窓の幅)を示すパラメータである。また、2次元ガボール関数GB(x、y)は、波の方向を示すパラメータ(方向パラメータ)として角周波数u0の偏角φを有している。
【0034】
【数1】
【0035】
上記のような2次元ガボール関数GB(x、y)を利用したガボールフィルタを用いて、入力画像データをフィルタリングすることにより、入力画像の周波数特性やテクスチャ(模様)方向特性を抽出することができる。具体的には、入力画像において、方向パラメータ(偏角φ)と平行なテクスチャ方向を有すると共に、周波数パラメータ(角周波数u0)に近い周波数特性を有する領域が感度良く抽出される。
【0036】
本実施形態では、上述したように、方向パラメータ(角周波数u0の偏角φ)の異なるn個のガボールフィルタGF1〜GFnを予め用意して記憶装置2bに記憶しておく。ここで、偏角φが等角度間隔で異なるようにガボールフィルタGF1〜GFnを用意する。例えば、n=72とすると、偏角φはπ/72(rad)=2.5°ずつ異なることになる。つまり、ガボールフィルタGF1の方向パラメータφ1を0(rad)と設定し、ガボールフィルタGF2の方向パラメータφ2をπ/72(rad)=2.5°と設定し、ガボールフィルタGF3の方向パラメータφ3を2π/72(rad)=5°と設定し、以下同様に、ガボールフィルタGFnの方向パラメータφnを71π/72(rad)=177.5°と設定する。
なお、nの値は入力画像の解像度や信号処理装置2cの処理能力に応じて適宜変更すれば良いが、例えばデジタル処理を考慮して8の倍数である32〜96の範囲で設定することが望ましい。
【0037】
一方、各ガボールフィルタGF1〜GFnの周波数パラメータ(角周波数u0)と減衰パラメータ(標準偏差σ)は、各フィルタ共通の所定値に設定する。ここで、周波数パラメータは、検出対象の通行痕跡の幅に応じて設定する。例えば、波の波長(周期)が視野上で1.5度〜4度となるように角周波数u0を計算して設定する。通路の撮影エリアが奥行き方向に10mから20mの領域である場合、角周波数u0が3度の視野に相当するように設定すると、撮影対象上での波の山から谷間での距離は25cm〜50cmとなり、車輪よりやや大きいサイズとなり、複数回車輪が通過した結果にできる車輪跡に近いサイズとなる。つまり、このように周波数パラメータを設定することにより、車輪跡のような轍が通路として感度良く抽出されやすくなる。また、減衰パラメータであるσは1〜0.5波長程度で設定すれば良い。
【0038】
信号処理装置2cは、例えばCPU(Central Processing Unit)であり、記憶装置2bから読み出した画像処理用プログラムPGに従って、入力画像データを基に所定の画像処理(フィルタリング処理、模様方向特定処理、強エッジ領域抽出処理、痕跡領域検出処理)を行うことにより、入力画像に存在する轍などの通行痕跡を検出し、その通行痕跡検出結果を経路計画装置5に出力するものである。
【0039】
ここで、フィルタリング処理とは、記憶装置2bからガボールフィルタGF1〜GFnを読み出し、各ガボールフィルタGF1〜GFnを用いて入力画像における検出対象領域をフィルタリングすることにより、検出対象領域についての各方向毎(φ1〜φn)の強度分布データを生成する処理を指す。また、模様方向特定処理とは、フィルタリング処理によって生成された強度分布データを基に検出対象領域における各画素のテクスチャ(模様)方向を特定する処理を指す。
【0040】
また、強エッジ領域抽出処理とは、検出対象領域に存在する強エッジ領域を抽出する処理を指す。また、痕跡領域検出処理とは、模様方向特定処理による各画素のテクスチャ方向の特定結果と、強エッジ領域抽出処理による強エッジ領域抽出結果とを基に所定の統計処理を行うことにより、検出対象領域において通行痕跡が存在すると推定される痕跡領域を検出する処理を指す。なお、これらの各種画像処理の詳細については後述する。
【0041】
RAM2dは、信号処理装置2cが各種画像処理を実行する上で必要なデータ(強度分布データやテクスチャ方向の特定結果、強エッジ領域抽出結果、統計データ等)や、上記の入力画像データを一時的に記憶するために使用される書換え可能な揮発性メモリである。
【0042】
以上が画像処理装置2の説明であり、以下では図1に戻って説明を続ける。
3次元計測装置3(3次元計測手段)は、車両Cの進行方向の3次元形状を計測するレーザ・レンジ・ファインダ(LRF)であり、車両Cの上部中央において下向きに所定角度傾けて設置されている。このようなLFRは、X軸方向の首振り(スイング)機構を有しており、図1(b)に示す走査平面SP上を走査するようにレーザ光Lを照射し、そのレーザ光Lの照射点P1、P2、・・・、Pnまでの距離データd1、d2・・・、dnと、そのスイング角度データα1、α2、・・・、αnとを、3次元形状計測結果として地図作成装置4に出力する。
【0043】
なお、市販されているLRFの計測可能範囲は、悪条件環境下において30m程度であるため、3次元計測装置3として使用するLFRも30m先の3次元形状を計測できるように設置角度や設置高さ等を設定することが望ましい。また、LFRをZ軸方向にスイングさせながらレーザ光Lを走査することで3次元形状を計測するようにしても良い。また、3次元計測装置3としてはLRF以外の装置、例えばステレオカメラ等を用いても良い。
【0044】
地図作成装置4(地図作成手段)は、例えば3次元マップ生成装置(3次元マップ作成専用のPC(Personal Computer))であり、3次元計測装置3から得られる3次元形状計測結果(距離データd1、d2・・・、dn、スイング角度データα1、α2、・・・、αn)を基に、車両Cの進行方向の地図(3次元マップ)を作成し、その3次元マップデータを経路計画装置5に出力する。なお、LFRから得られる距離データ及びスイング角度データから3次元マップを作成する手法は既に公知であるため、本実施形態では詳細な説明を省略する。
また、地図作成装置4としては、3次元マップ生成装置以外にも、荒さ・起伏マップ作成装置を利用することもでき、この場合には起伏の量を計測した地面の高さを微分した値を、上空から見た鳥瞰図的な座標にプロットして地図を作成する。
【0045】
経路計画装置5(経路計画手段)は、例えば経路計画専用のPC(Personal Computer)であり、画像処理装置2から得られる通行痕跡検出結果と、地図作成装置4から得られる3次元マップデータとを基に、車両Cの移動経路を計画し、その経路計画データを運動制御装置9に出力する。なお、この移動経路の計画手法の詳細については後述する。
【0046】
GPS受信機6(位置情報取得手段)は、車両Cの上部に設置されており、GPS(Global Positioning System:全地球測位システム)を利用して車両Cの位置に関する情報として経度、緯度、高度を取得し、その取得結果を車両位置データとして位置・姿勢推定装置8に出力する。ジャイロ7(姿勢情報取得手段)は、車両Cの所定箇所に設置された3軸ジャイロセンサであり、車両CのX軸回り(ピッチ)の回転角度及び角速度と、Y軸回り(ロール)の回転角度及び角速度と、Z軸回り(ヨー)の回転角度及び角速度とを、車両Cの姿勢情報として取得し、その取得結果を車両姿勢データとして位置・姿勢推定装置8に出力する。
【0047】
位置・姿勢推定装置8(位置・姿勢推定手段)は、例えば位置・姿勢推定専用のPC(Personal Computer)であり、GPS受信機6から得られる車両位置データと、ジャイロ7から得られる車両姿勢データと、後述するアクチュエータ10から得られる車両Cの移動量に関するフィードバックデータとを基に、車両Cの現在位置及び姿勢を推定し、その推定結果を現在位置・姿勢データとして運動制御装置9に出力する。
【0048】
運動制御装置9(運動制御手段)は、例えば車両Cの運動制御専用のPC(Personal Computer)であり、経路計画装置5から得られる経路計画データと、位置・姿勢推定装置8から得られる現在位置・姿勢データとを基に、車両Cが計画された移動経路に沿って移動するように、且つ移動経路を中心とする所定の許容範囲内を移動するようにアクチュエータ10を制御する。
【0049】
アクチュエータ10は、車両Cの車輪を駆動するためのモータ、ステアリングを駆動するためのモータ、ブレーキを駆動するためのモータや、これらモータの駆動状態を計測するセンサ(例えばステアリングの回転量を計測するポテンショメータやロータリエンコーダ、車輪の回転量を計測するロータリエンコーダ等)、及びそれらとフィードバックループを構成しつつ、運動制御装置9の制御によって各種モータに駆動電圧または電流を供給するモータアンプなどから構成されている。また、各種センサによって計測されたデータは、車両Cの移動量に関するフィードバックデータとして位置・姿勢推定装置8にフィードバックされる。
【0050】
なお、本実施形態では、画像処理装置2、地図作成装置4、経路計画装置5、位置・姿勢推定装置8及び運動制御装置9をそれぞれ独立したPCによって構成した場合を例示したが、各装置の機能を1つのPCに集約するような構成としても良い。
【0051】
次に、上記のように構成された車両C(特に移動体制御装置)の動作について詳細に説明する。
【0052】
(通行痕跡検出動作)
まず、画像処理装置2による通行痕跡検出動作について説明する。移動体制御装置の動作中において、撮影装置1a、1bは、所定のカメラレートで車両Cの進行方向を撮影し、撮影画像を示す信号(画像信号)を画像処理装置2に出力する。画像処理装置2に入力された画像信号は、A/Dコンバータ2aによって入力画像データに変換された後、信号処理装置2cに転送される。
【0053】
ここで、車両Cは川原を走行しており、撮影装置1a、1bから図3(a)に示すような轍が通行痕跡として写り込んだ撮影画像が得られたものと想定する。また、以下では、図3(b)に示すように、撮影画像の水平方向をX軸方向、垂直方向をY軸方向とし(このXY軸は図1に示すXYZ直交座標系とは無関係である)、撮影画像の座標(X、Y)=(0、0)〜(639、479)の各画素に対応する輝度データをB(0、0)〜B(639、479)と表記する。つまり、入力画像データは、これら輝度データB(0、0)〜B(639、479)の集合である。このような入力画像データは、信号処理装置2cによってRAM2dに一旦記憶される。
【0054】
図4は、信号処理装置2cの動作フローチャートである。この図4に示すように、まず、信号処理装置2cは、RAM2cから入力画像(撮影画像)における検出対象領域に相当する入力画像データを読み出すと共に、記憶装置2bからガボールフィルタGF1〜GFnを読み出し、各ガボールフィルタGF1〜GFnを用いて入力画像データをフィルタリングすることにより、各方向の強度分布データを生成する(ステップS1:フィルタリング処理)。
【0055】
本実施形態では、入力画像の全領域が上記の検出対象領域に設定されている場合を例示して説明する。つまり、RAM2dから入力画像の全領域に相当する入力画像データが読み出される。なお、入力画像から水平線を抽出可能である場合には、その水平線より下の領域(つまり地面の領域)を検出対象領域として設定しても良い。このように、確実に通行痕跡が存在すると推定される領域のみを検出対象領域とすることにより、以下で説明する各種画像処理の処理負荷を軽減でき、処理時間を短縮することができる。
【0056】
以下、図5を参照して上記ステップS1について具体的に説明する。図5(a)に示すように、信号処理装置2cは、ガボールフィルタGF1〜GFnを用いたフィルタリング処理として、入力画像において、ある画素を中心画素とする畳み込み領域に含まれる各画素の輝度データと、ガボールフィルタ、つまり2次元ガボール関数GB(x、y)との畳み込み演算を行い、その演算結果をガボールフィルタの方向パラメータφに対応する中心画素の強度として取得する。
【0057】
つまり、信号処理装置2cは、まず、ある画素を中心画素とする畳み込み領域に含まれる各画素の輝度データとガボールフィルタGF1との畳み込み演算を行い、その演算結果をガボールフィルタGF1の方向パラメータφ1に対応する中心画素の強度として取得する。ここで、各ガボールフィルタGF1〜GFnを、実数部のフィルタと虚数部のフィルタとに分けて作成しておくことにより、実数部のフィルタを用いて得られた強度と、虚数部のフィルタを用いて得られた強度とをそれぞれ2乗して加算し、その加算値の平方根を求めることで最終的な強度を取得することができる。
【0058】
続いて、信号処理装置2cは、ガボールフィルタGF2に切り替えて同様に畳み込み演算を行い、その演算結果をガボールフィルタGF2の方向パラメータφ2に対応する中心画素の強度として取得する。以下、同様に、信号処理装置2cは、ガボールフィルタを順次切り替えながら畳み込み演算を行うことにより、その演算結果を各ガボールフィルタGF1〜GFnの方向パラメータφ1〜φnに対応する中心画素の強度として取得する。
【0059】
信号処理装置2cは、上記のようなフィルタリング処理を、入力画像の座標(0、0)〜(639、479)の各画素のそれぞれについて行うことにより、図5(b)に示すような各方向の強度分布データを生成してRAM2dに記憶する。なお、図5(b)では、座標(0、0)〜(639、479)の各画素のそれぞれを中心画素として得られた強度データをI(0、0)〜I(639、479)として表記している。
【0060】
なお、図5(a)からわかるように、入力画像の端部に近い画素を中心画素とする場合、畳み込み領域が入力画像からはみ出てしまうため、この場合には予め設定した輝度データ(例えば白データ等)を入力画像からはみ出ている畳み込み領域の輝度データとして使用すれば良い。また、畳み込み領域の大きさは、検出対象の通行痕跡の大きさに応じて適宜変更すれば良いが、例えば幅25cm〜50cmの車輪跡に相当する轍を検出対象とする場合には、その幅に相当する画素数(32画素)以上の大きさ、例えば50×50画素の大きさとすることが望ましい。
【0061】
次に、上記のようなステップS1が終了すると、信号処理装置2cは、RAM2dから各方向の強度分布データを読み出し、それら強度分布データを基に入力画像における各画素のテクスチャ(模様)方向を特定する(ステップS2:模様方向特定処理)。具体的には、例えば座標(0、0)の画素に着目した場合、各方向の強度分布データの中から強度データI(0、0)が最も大きいものを探索し、その最大の強度データI(0、0)を含む強度分布データに対応する方向を、座標(0、0)の画素のテクスチャ方向として特定する。例えば、方向φ3に対応する強度分布データに最大の強度データI(0、0)が含まれている場合、方向φ3が座標(0、0)の画素のテクスチャ方向となる。
【0062】
信号処理装置2cは、上記のようなテクスチャ方向の特定処理を、座標(0、0)〜(639、479)の各画素について行うことにより、各画素のテクスチャ方向を特定し、その特定結果をRAM2dに記憶する。図6(a)は各画素のテクスチャ方向の特定結果であり、図6(b)は入力画像とテクスチャ方向の特定結果とを重ねて表示したものである。なお、この図6では、説明の便宜上、テクスチャ方向の特定結果を、テクスチャ方向を指し示すベクトル線で表しているが、実際にはテクスチャ方向の特定結果は、その方向を表す値(例えばテクスチャ方向がφ1であればφ1の値)によって構成されている。
【0063】
不整地上に存在する轍などの通行痕跡は、車両が複数回その場所を走行することにより、地面や草むらが踏み起こされたり、踏み固められたりした結果残るものであるため、図3に示すように、その痕跡は入力画像上において不連続且つ不明瞭な帯状やスジ状の疎らな領域として観察される。そこで、上述した入力画像の波の成分を抽出する機能を有するステップS1及びステップS2を実施することより、通行痕跡を形成する模様の間隔は不定でなだらかではあるが、通行痕跡の進行方向と平行な波の成分として捕らえることができる。
【0064】
次に、上記のようなステップS2が終了すると、信号処理装置2cは、入力画像データを基に所定のエッジ検出処理を行うことにより、入力画像に存在する強エッジ領域を抽出し、その抽出結果をRAM2dに記憶する(ステップS3:強エッジ領域検出処理)。ここで、エッジ検出処理としては公知の技術を採用することができる。例えば、X軸方向のソベルフィルタとY軸方向のソベルフィルタを適用して、それらの強度の2乗和が一定の閾値を超えた画素を探索し、その閾値を超えた画素から一定距離以内の領域を強エッジ領域として抽出する。
【0065】
次に、上記のようなステップS3が終了すると、信号処理装置2cは、模様方向特定処理による各画素のテクスチャ方向の特定結果と、強エッジ領域抽出処理による強エッジ領域抽出結果とを基に所定の統計処理を行うことにより、入力画像において通行痕跡が存在すると推定される痕跡領域を検出する(ステップS4:痕跡領域検出処理)。具体的には、信号処理装置2cは、ステップS4における統計処理として、投票処理(ステップS4a)、消失点特定処理(ステップS4b)、有効投票画素抽出処理(ステップS4c)及び痕跡領域特定処理(ステップS4d)を行う。以下、それぞれの処理について詳細に説明する。
【0066】
<投票処理>
まず、信号処理装置2cは、ステップS4aの投票処理として、入力画像における各画素のそれぞれを投票元画素として順次選択し、当該投票元画素を起点としそのテクスチャ方向に平行な直線上に存在する画素に対して投票を行う。具体的には、信号処理装置2cは、まず、図7に示すように、RAM2dのメモリ空間上に、投票回数を集計するための投票集計領域を入力画像と同サイズ(つまり、640×480画素数分の投票回数を集計可能なサイズ)で確保する。
【0067】
そして、信号処理装置2cは、図8(a)に示すように、ある画素を投票元画素として選択した場合、模様方向特定処理による各画素のテクスチャ方向の特定結果から投票元画素のテクスチャ方向を把握し、投票元画素を起点としそのテクスチャ方向に平行な直線上に存在する画素(投票対象画素)に対して投票を行い、その投票結果を図8(b)に示すようにRAM2d上の投票集計領域に反映する。つまり、図8(b)からわかるように、投票集計領域において投票対象画素に対応する記憶領域に投票値「1」が加算される。
【0068】
信号処理装置2cは、上記のような投票処理を各画素について行い、その投票結果を順次RAM2d上の投票集計領域に反映する。ここで、信号処理装置2cは、上記のような投票処理を行う際、強エッジ領域抽出処理によって抽出された強エッジ領域の周辺(たとえば2画素以内)に含まれる画素を投票元画素から除外する。この理由については後述する。
【0069】
なお、上述の投票処理では、投票元画素を起点としそのテクスチャ方向に平行な直線上に存在する投票対象画素に対して投票を行う場合を説明したが、これに限らず、投票元画素を起点としてそのテクスチャ方向に沿うように設定された投票対象領域に含まれる画素(投票対象画素)に対して投票を行うようにしても良い。この投票対象領域は、例えば図9(a)に示すように、テクスチャ方向に平行な帯状(長方形状)のものでも良いし、また、図9(b)に示すように、円弧状あるいは三角形状のものでも良い。
【0070】
ここで、図9(a)に示すような帯状の投票対象領域を設定する場合には、上述したテクスチャ方向に平行な直線上に存在する投票対象画素に対して投票を行う場合と同様に、投票対象領域に含まれる投票対象画素のそれぞれに対して平等に投票すれば良い。つまり、RAM2d上の投票集計領域において、各投票対象画素に対応する記憶領域に平等に投票値「1」を加算する。
【0071】
一方、図9(b)に示すような円弧状あるいは三角形状の投票対象領域を設定する場合には、投票元画素と投票対象画素との間の距離に応じて投票値を変更することが望ましい。具体的には、投票元画素から遠い距離に位置する投票対象画素の投票値を小さくする(例えば、投票値=1/投票元画素からの距離)。つまり、投票元画素に近い位置の投票対象画素の投票値は「1」または「1」に近い値となるが、投票元画素から遠い位置の投票対象画素の投票値は「0.5」等の小数点以下の値となる。さらには、投票対象領域において同じ距離に位置する投票対象画素の総投票値が「1」となるように各投票対象画素の投票値を決定することが望ましい。つまり、例えば同じ距離に位置する投票対象画素が4つ存在すると仮定すると、それら4つの各投票対象画素に対する投票値をそれぞれ「0.25」とする。
【0072】
このように円弧状あるいは三角形状の投票対象領域を設定した場合に、投票元画素と投票対象画素との間の距離に応じて投票値を変更する理由は、投票元画素から遠い位置にある投票対象画素は、他の投票対象領域と重なり合う確率が高く、近い位置にある投票対象画素と比べて投票されやすい傾向にあり、後述する消失点を正確に特定することが困難となるためである。また、円弧状あるいは三角形状の投票対象領域を設定した場合、その中心角度(投票元画素を起点とする角度)は、ガボールフィルタGF1〜GFnの方向パラメータφの変化分(つまりπ/72(rad)=2.5°)と同一にすることが望ましい。
【0073】
<消失点特定処理>
続いて、信号処理装置2cは、ステップS4bの消失点特定処理として、図10に示すように、投票処理による投票結果(つまり、RAM40上の投票集計領域に集計された投票回数)を基に、投票回数が最も大きい画素を入力画像における消失点として特定する。轍などの通行痕跡の波模様は車両Cから見ると完全に平行ではないが同じ消失点を共有していると推測される。このため、上記のような投票処理によって消失点と推測される位置(画素)に投票を行い、統計的に通行痕跡と平行している波模様が多い検出結果を活用することにより、消失点を特定することができる。
【0074】
ここで、入力画像において通行痕跡以外に、例えば木や電柱、看板などのコントラストの高い物体が写り込んでいた場合、それらの模様は上記の投票回数の集計に大きな影響を及ぼすことになる。そこで、上述したように、投票処理を行う際には、強エッジ領域抽出処理によって抽出された強エッジ領域に含まれる画素を投票元画素から除外することにより、木や電柱、看板などのコントラストの高い物体の延長線上が消失点として特定されることを防止することができる。
【0075】
<有効投票画素抽出処理>
続いて、信号処理装置2cは、ステップS4cの有効投票画素抽出処理として、消失点の投票元画素を有効投票画素として抽出する。つまり、消失点を向いているテクスチャ方向を有する画素が有効投票画素として抽出されることになる。図11に、有効投票画素の抽出結果を示す。図11において白領域が有効投票画素の集合領域である。
【0076】
<痕跡領域特定処理>
続いて、信号処理装置2cは、ステップS4dの痕跡領域特定処理として、図12に示すように、入力画像において消失点を頂点とする領域(この領域は円弧状でも良いし、三角形状でも良い)を複数の小領域に分割し、各小領域の内、有効投票画素の比率が所定の閾値を超えた小領域を通行痕跡が存在すると推定される痕跡領域として特定する。ここで、有効投票画素の比率は、小領域と有効投票画素との面積比率、つまり、小領域に含まれる有効投票画素数を小領域に含まれる全画素数で除算することで求めることができる。
【0077】
入力画像において轍などの通行痕跡が写り込んでいる領域では、周囲と比べて高い比率で消失点を向いているテクスチャを含んでいると推測される。そこで、上記のような有効投票画素抽出処理及び痕跡領域特定処理を行うことにより、入力画像において通行痕跡が存在する痕跡領域と他の領域とを判別することができる。
【0078】
なお、痕跡領域は轍などの通行痕跡に沿って検出されるものであるので、隣接する痕跡領域は、ある規則性を持って連続的または断続的に連なっているはずである。そこで、入力画像において、ある規則性を持った痕跡領域の集まりから孤立している痕跡領域はノイズとして捉え、そのような孤立領域は痕跡領域から除去することにより、通行痕跡の誤検出を防止することができる。
【0079】
ところで、上記説明は、図3に示すように、通行痕跡が水平線に向かって一直線に延びているような入力画像が得られた場合を想定したものである。しかしながら、図13に示すように、入力画像中の通行痕跡がカーブしているような場合には、入力画像の奥行き毎に消失点が水平方向に変化することになる。このような場合、上述したように入力画像の全領域(検出対象領域の全領域)を一度に処理すると、正確な消失点を得ることができなくなる。
【0080】
そこで、このような問題に対処するために、上記のステップS4の処理(投票処理、消失点特定処理、有効投票画素抽出処理及び痕跡領域特定処理)を、図13に示すように、検出対象領域(ここでは入力画像の全領域)を奥行き方向に沿って複数に分割して得られる副検出対象領域ごとに行うようにしても良い。このように、副検出対象領域毎に消失点を求め、その対応する奥行きごとに消失点に適合する有効投票画素を求めることで、それぞれの奥行きに対応した消失点と痕跡領域の切り出しを行うことができる。
【0081】
ここで、2次元ウェーブレットフィルタを用いた場合、入力画像上で強い方向性を有する領域が轍などの痕跡領域として検出されるため、轍の外に生えている草や樹木、人工物の平行な特徴を誤って検出してしまう可能性がある。そこで、上記のように、奥行き方向に沿って複数に分割して得られる副検出対象領域ごとに痕跡領域の特定を行う場合、以下に説明する処理を加えることで検出精度の向上を図ることができる。
【0082】
実験の結果、轍検出(痕跡領域検出)の精度は各地点までの奥行き方向の距離に依存し、手前ほど精度が高いことが判明した。また、検出済みの轍(痕跡領域)と連続した領域はやはり轍(痕跡領域)である可能性が大きいと考えられる。そこで、入力画像を手前から奥へ数段階に分割し、最も手前側の副検出対象領域に存在する轍(痕跡領域)を最初に検出して、それを基準に以遠の轍(痕跡領域)を検出することとする。
【0083】
図14に示すように、まず、奥行き方向の最も手前側の副検出対象領域(ここでは第1副検出対象領域W1とする)について、上記実施形態で説明したステップS4の処理(投票処理、消失点特定処理、有効投票画素抽出処理及び痕跡領域特定処理)を行うことで第1副検出対象領域W1における痕跡領域特定結果を得る。なお、図14では、痕跡領域の特定結果を第1副検出対象領域W1の消失点に向かう連続した直線として表している。
【0084】
続いて、次段の副検出対象領域(ここでは第2副検出対象領域W2とする)について上記ステップS4の処理を行うが、この時の投票処理では、現在着目している副検出対象領域(つまり第2副検出対象領域W2)において、図14に示すように、以前着目していた副検出対象領域(つまり第1副検出対象領域W1)について実施された痕跡領域特定処理によって特定された痕跡領域の終端側に、投票の重み付けを大きくした投票重み付け領域Waを設定する。このような投票重み付け領域Waは、痕跡領域の特定結果を表す各直線上のそれぞれの終端部を中心に設定した所定サイズの矩形領域から構成されている。この矩形領域の1つ当たりのサイズは、縦:0.08×画素高さ、横:0.03×画素幅、程度とする。
【0085】
このように投票重み付け領域Waを設定した第2副検出対象領域W2について上記ステップS4の投票処理を行う場合、投票重み付け領域Waに存在する投票対象画素に対する投票には通常の3倍程度の重み付けを行う。例えば、1段目の第1副検出対象領域W1の投票処理において投票対象画素に対する投票値を「1」とした場合、第2副検出対象領域W2の投票重み付け領域Waに存在する投票対象画素に対する投票値は「3」とする。
【0086】
このような第2副検出対象領域W2についてステップS4の投票処理、消失点特定処理、有効投票画素抽出処理及び痕跡領域特定処理を行うことにより、前段の第1副検出対象領域W1について特定された痕跡領域の終端が、次段の第2副検出対象領域W2の痕跡領域の起点となりやすくなる。言い換えれば、前段の第1副検出対象領域W1の痕跡領域と次段の第2副検出対象領域W2の痕跡領域とが連続した痕跡領域として検出されやすくなり、入力画像中に轍などと平行に生えた草や樹木、人工物等が存在する場合であっても、精度良く轍などの痕跡領域を検出することができる。
【0087】
上記のように第2副検出対象領域W2について痕跡領域の特定結果を得た後は、次段(3段目)の副検出対象領域について第2副対象領域W2と同様の処理を行う。なお、3段目の副検出対象領域では、投票重み付け領域を構成する矩形領域の1つ当たりのサイズを、2段目の第2副検出対象領域W2の半分、つまり、縦:0.04×画素高さ、横:0.015×画素幅、程度とする。
【0088】
また、3段目の副検出対象領域について上記ステップS4の投票処理を行う場合、投票重み付け領域に存在する投票対象画素に対する投票には通常の5倍程度の重み付けを行う。例えば、1段目の第1副検出対象領域W1の投票処理において投票対象画素に対する投票値を「1」とした場合、3段目の副検出対象領域の投票重み付け領域に存在する投票対象画素に対する投票値は「5」とする。このように奥行き方向に沿った位置に応じて投票重み付け領域の大きさ及び投票の重み付け量を変化させることにより、より精度良く痕跡領域の連続性を確保することができる。
【0089】
以降、同様な処理を奥行き方向の最も遠い副検出対象領域まで行うことにより、各段の副検出対象領域で特定された痕跡領域が連続した痕跡領域として精度良く検出され、轍などの車両の走行痕を正確に検出することが可能となる。なお、4段目以降の副検出対象領域では、投票重み付け領域を構成する矩形領域の1つ当たりのサイズ及び投票の重み付け量を3段目の副検出対象領域と同一にすることが好ましい。
【0090】
図15(a)は、投票重み付け領域を使用しない場合における痕跡領域の特定結果を示すものである。この図15(a)に示すように、投票重み付け領域を使用しない場合、本来、痕跡領域とすべき領域とは異なる領域を痕跡領域として特定してしまっていることがわかる。また、図15(b)は、投票重み付け領域を使用した場合における痕跡領域の特定結果を示すものである。この図15(b)に示すように、投票重み付け領域を使用した場合、本来、痕跡領域とすべき領域を正しく痕跡領域として特定していることがわかる。
【0091】
なお、例えば、現在着目している副検出対象領域(例えば第2副検出対象領域W2)について痕跡領域特定処理が終了した後に、以前着目していた副検出対象領域(第1副検出対象領域W1)において、第2副検出対象領域W2について実施された痕跡領域特定処理によって特定された痕跡領域の始端側に、投票の重み付けを大きくした投票重み付け領域を設定し、当該第1副検出対象領域W1についてステップS4の投票処理、消失点特定処理、有効投票画素抽出処理及び痕跡領域特定処理を再実施することで、当該第1副検出対象領域W1についての痕跡領域特定結果を修正することも可能である。
【0092】
以上のように、画像処理装置2における信号処理装置2cは、上述した画像処理を実施することによって、入力画像において通行痕跡が存在すると推定される痕跡領域を検出し、その検出結果を通行痕跡検出結果として経路計画装置5に出力する。なお、上述した画像処理を、カメラレートで撮影装置1a、1bから送られる撮影画像毎に実施する必要はなく、毎秒2枚から10枚程度の撮影画像について実施すれば良い。
【0093】
(3次元マップ作成動作)
続いて、地図作成装置4による3次元マップ作成動作について説明する。移動体制御装置の動作中において、3次元計測装置3(LRF)は、図1(b)に示す走査平面SP上を走査するようにレーザ光Lを照射し、そのレーザ光Lの照射点P1、P2、・・・、Pnまでの距離データd1、d2・・・、dnと、そのスイング角度データα1、α2、・・・、αnとを、3次元形状計測結果として地図作成装置4に出力する。ここで、図16(a)に示すように、車両Cを走行させながらレーザ光Lを走査することで3次元形状を計測しても良いし、LFRをZ軸方向にスイングさせながらレーザ光Lを走査することで3次元形状を計測しても良い。
【0094】
地図作成装置4は、3次元計測装置3から得られる3次元形状計測結果(距離データd1、d2・・・、dn、スイング角度データα1、α2、・・・、αn)を基に、図16(b)に示すように、車両Cの進行方向の3次元マップを作成し、その3次元マップデータを経路計画装置5に出力する。なお、LFRから得られる距離データ及びスイング角度データから3次元マップを作成する手法は既に公知であるため、本実施形態では詳細な説明を省略する。
【0095】
(経路計画動作)
続いて、経路計画装置5による経路計画動作について説明する。経路計画装置5は、画像処理装置2から得られる通行痕跡検出結果と、地図作成装置4から得られる3次元マップデータとを基に、轍(通行痕跡)に沿った方向、且つ轍から一定以内の距離を保つ範囲の中という条件の下、周囲の3次元形状から最適な移動経路を計画する。
【0096】
より具体的には、経路計画装置5は、移動経路上の起伏、および移動経路上の各地点でのステアリング量など走行の安定性を低下させる要因をその度合いに応じたコスト値に変換し、速度・回転半径などの車体性能の制約条件を満たしつつ、そのコストが最小となるように移動経路を計画する。もしくは、移動経路上の起伏度合いとステアリングからそこで許容されている最大速度を求め、その移動経路上での最大速度に近くなるような加減速を行うことをシミュレーションし、その到達時間が最短となる移動経路を計画しても良い。
【0097】
また、作成された3次元マップから周囲の3次元形状の変化を監視し、大きな変化がない場合には、近傍は再計画を行わず、遠方(たとえば計画を行う範囲の1/2以上遠方)のみを再計画するようにしても良い。これにより、近傍の軌道が再計画により細々変動したり軌道が振動したりして移動経路が不安定になることを防ぐことができる。一方、3次元マップや轍などの通行痕跡検出結果が大きく変化した場合には、全経路での再計画を行うことが望ましい。さらに、轍に沿った範囲で安定に走行できる条件での計画ができなかった場合には、モードを轍を考慮しないモード(つまり画像処理装置2から得られる通行痕跡検出結果を無視するモード)に切り替え、従来技術と同様に、3次元マップのみを用いて最適な移動経路を計画することが望ましい。
以上のような手法により、経路計画装置5は、図17に示すような車両Cの移動経路を計画し、その経路計画データを運動制御装置9に出力する。
【0098】
(車両Cの現在位置及び姿勢の推定動作)
続いて、位置・姿勢推定装置8による車両Cの現在位置及び姿勢の推定動作について説明する。移動体制御装置の動作中において、GPS受信機6は、車両Cの位置に関する情報として経度、緯度、高度を取得し、その取得結果を車両位置データとして位置・姿勢推定装置8に出力する。また、ジャイロ7は、車両CのX軸回り(ピッチ)の回転角度及び角速度と、Y軸回り(ロール)の回転角度及び角速度と、Z軸回り(ヨー)の回転角度及び角速度とを、車両Cの姿勢情報として取得し、その取得結果を車両姿勢データとして位置・姿勢推定装置8に出力する。さらに、アクチュエータ10の各種センサによって計測されたデータは、車両Cの移動量に関するフィードバックデータとして位置・姿勢推定装置8にフィードバックされる。
【0099】
位置・姿勢推定装置8は、GPS受信機6から得られる車両位置データと、ジャイロ7から得られる車両姿勢データと、アクチュエータ10から得られる車両Cの移動量に関するフィードバックデータとに加えて、GPS受信機6で受信可能な衛星数から求まる車両位置の計測誤差、及び不整地で事前に測定した車輪の滑りから求まる計測誤差に基づいて、尤もらしい車両Cの現在位置及び姿勢を推定し、その推定結果を現在位置・姿勢データとして運動制御装置9に出力する。なお、この推定にはカルマンフィルタやパーティクルフィルタを用いることで、より高精度に車両Cの現在位置及び姿勢を求めることが可能になる。
【0100】
(車両Cの運動制御動作)
続いて、運動制御装置9による車両Cの運動制御動作について説明する。運動制御装置9は、経路計画装置5から得られる経路計画データと、位置・姿勢推定装置8から得られる現在位置・姿勢データとを基に、車両Cが計画された移動経路に沿って移動するように、且つ移動経路を中心とする所定の許容範囲内を移動するようにアクチュエータ10を制御する。
【0101】
具体的には、まず、計画された移動経路及び速度となるように、車両Cの現在位置及び姿勢を基に計画された移動経路に対するズレ量を加算し、所定の許容範囲内で移動経路に沿うように車両Cの目標旋回半径を決定する(移動経路に十分近ければ、移動経路の旋回半径を目標旋回半径に決定する)。そして、目標旋回半径の決定後、予め作成しておいたステアリング角・速度と旋回半径とを対応付けるテーブルに基づいて、現在速度及び目標旋回半径から実施すべきステアリング角を決定する。運動制御装置9は、上記のように決定したステアリング角となるようにアクチュエータ10に対してステアリングの回転量を指示する。
【0102】
一方、現在の旋回半径、及び近傍(10m程度)の移動経路の旋回半径の最も厳しい(半径の小さい)値から車両Cが安全に旋回するための最大速度を求め(車両Cの物理モデルと砂地などの摩擦係数を用いてシミュレーションを行って事前に求めておく)、最大速度と計画された速度との小さい方を車両Cの目標速度とする。運動制御装置9は、上記のように決定した目標速度と現在の速度とを基に、PID制御などを用いて制御値を決定し、アクチュエータ10の車輪を駆動するためのモータを制御する。
なお、移動経路に対するズレ量が大きくなった場合には、経路計画装置5に対して移動経路の再計画を指示し、以前の移動経路に依存せずに、現在位置からスムーズな移動経路を計画することが望ましい。
【0103】
以上説明したような運動制御装置9による運動制御動作によって、図18に示すように、車両Cは計画された移動経路に沿って移動するように、且つ移動経路を中心とする所定の許容範囲内を移動するように制御されることになる。
【0104】
以上のように、本実施形態によれば、不整地のように全体的に平坦・なだらかな地形であっても、そこある道のような通行痕跡(轍、ふみ跡など)を検出することが可能である。そして、不整地における通行痕跡は、実際に以前から何度か車両が通行していた結果生じるものであり、その延長線上は継続して通行可能なエリアになっている可能性が極めて高い。このような通行痕跡を使って計測エリア外の通行可能性が高い方向を予測することで、計測エリア外も含めて効率的な移動経路を計画することが可能となる。
【0105】
なお、本発明は上記実施形態に限定されず、以下のような変形例が考えられる。
(1)上記実施形態では、各ガボールフィルタGF1〜GFnの周波数パラメータ(角周波数u0)を固定値とする場合を例示した。これは、通路の撮影エリアが奥行き方向に10mから20mの領域である場合、角周波数u0が視野中の3度に相当するように設定すると、撮影対象上での波の山から谷間での距離は25cm〜50cmとなり、車輪よりやや大きいサイズとなり、複数回車輪が通過した結果にできる車輪跡に近いサイズとなるためである。しかしながら、図3の入力画像からわかるように、轍などの通行痕跡の幅は、奥行き方向の位置によって変化する。そこで、入力画像における奥行き方向の位置に応じて、各ガボールフィルタGF1〜GFnの周波数パラメータを変更することが望ましい。
【0106】
具体的には、記憶装置2bに、入力画像における奥行き方向の位置に応じた周波数パラメータを有すると共に方向パラメータの異なるガボールフィルタを予め記憶しておき、信号処理装置2cに、記憶装置2bから検出対象領域におけるフィルタリング対象位置(つまり図5中の中心画素の位置)に対応する周波数パラメータを有するガボールフィルタを読み出してフィルタリング処理する機能を持たせれば良い。
【0107】
(2)上記実施形態では、空間周波数フィルタとして2次元ガボールフィルタを用いた場合を例示したが、これに限らず、その他の2次元ウェーブレットフィルタや2次元窓付きフーリエ変換を利用したフィルタなどを空間周波数フィルタとして用いても良い。
【0108】
(3)また、上記実施形態では、入力画像に存在する通行痕跡を検出する画像処理手法として、空間周波数フィルタを用いた場合を例示して説明したが、これに限定されず、通行痕跡を検出可能な画像処理手法であれば他の手法を採用しても良い。すなわち、撮影画像の中から幅を有する帯状の模様を通行痕跡として検出する手法、または、幅を有し、途中で断続的になっている帯状の模様が複数、3次元空間上で平行になっている部分を通行痕跡として検出する手法、或いは、幅を有し、途中で断続的になっている帯状の部分を撮影画像中から見つけ出し、最も多数の帯状部分が共有する消失点に向かっている帯状部分を通行痕跡として絞り込む手法であれば、他の手法を用いても良い。
【0109】
(4)上記実施形態では、移動体制御装置を搭載した移動体として車両Cを例示して説明したが、移動体はこれに限らず、ロボットなどの2足歩行をする移動体であっても本発明における移動体制御装置を適用することが可能である。
【符号の説明】
【0110】
C…車両、1a、1b…撮影装置、2…画像処理装置、3…3次元計測装置、4…3次元マップ作成装置、5…経路計画装置、6…GPS受信機、7…ジャイロ、8…位置・姿勢推定装置、9…運動制御装置、10…アクチュエータ、2a…A/Dコンバータ、2b…記憶装置、2c…信号処理装置、2d…RAM(Random Access Memory)、PG…通路検出プログラム、GF1〜GFn…ガボールフィルタ
【技術分野】
【0001】
本発明は、移動経路計画装置、移動体制御装置及び移動体に関する。
【背景技術】
【0002】
従来では、不整地における移動体(例えば車両)の制御手法として、以下の手法が採用されていた。まず、ステレオ画像計測やレーザレンジファインダ(LRF)等を用いて周囲の3次元形状を計測し、その計測結果に基づいて、大きな突起物等を障害物として検出する。そして、障害物から接触を避けられるだけの十分な距離を確保しつつ、且つ、経路上の起伏やカーブ等の減速要因・動揺など、運行を不安定にする要因が最小となるように車両の経路や速度等を計画し、その計画に従って車両の加減速、ステアリング角を求め、各アクチュエータによって車両のステアリング及び車輪の駆動を行う。
【0003】
なお、従来の移動体制御手法として、下記特許文献1には、周囲の3次元形状を計測して行動可能範囲(環境地図)を判断し、その行動可能範囲内で経路計画を行う手法が開示されている。また、下記特許文献2には、地形センサによるセンシングで経路探索地図を作成し、複数の経路候補の中から最も平坦な経路を走行上の最適経路として選択する手法が開示されている。また、下記特許文献3には、障害物の検出から各アクチュエータの制御に至るまでの自律走行制御システムの具体的な構成例が開示されている。さらに、下記特許文献4には、将来見通せる距離が短い場合の対処の例として、逐次再計画する手法が開示されている。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特開2005−092820号公報
【特許文献2】特開平07−064631号公報
【特許文献3】特開平11−212640号公報
【特許文献4】特開平11−007318号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
例えば、上述したステレオ画像計測やLRF等を用いて、図19(a)や(b)に示すような不整地の3次元形状を計測し、その計測結果に基づいて車両の経路計画を行う場合を想定する。この場合、図19(a)や(b)のような画像を撮影した場所において、周囲の3次元形状が正確に得られる近傍領域(ステレオ画像計測で十分な精度が出る範囲、またはLRFで計測している範囲で画像中の真ん中より下に相当する領域)は広い範囲で平坦であり、どのような経路もとりうるため、目標地点に向けて直行するような経路(図中の実線の矢印)を計画しがちである。
【0006】
ところが、そのように計画した経路に沿って車両を走行させ、ある一定以上の遠方まで進んだ地点で障害物が存在して行き止まりになり、急に前方が通行不能であることが判明した場合、障害物を回避して新たに経路を計画し直す必要が生じる。このように、従来の経路計画手法では、障害物を回避可能な理想的な経路(図中の破線の矢印)と、計画された経路とが必ずしも一致せず、経路計画効率の低下を招くという問題があった。
【0007】
本発明は、上述した事情に鑑みてなされたものであり、経路計画効率の向上を図ることが可能な移動経路計画装置、移動体制御装置及び移動体を提供することを目的とする。
【課題を解決するための手段】
【0008】
上記課題を解決するために、本発明では、移動経路計画装置に係る第1の解決手段として、移動体の進行方向を撮影する撮影手段と、前記撮影手段から得られる撮影画像を基に所定の画像処理を行うことにより、前記撮影画像に存在する通行痕跡を検出する画像処理手段と、前記画像処理手段によって検出された通行痕跡を基に前記移動体の移動経路を計画する経路計画手段とを備えることを特徴とする。
【0009】
また、移動経路計画装置に係る第2の解決手段として、上記第1の解決手段において、移動体の進行方向の3次元形状を計測する3次元計測手段と、前記3次元計測手段から得られる3次元形状の計測結果を基に地図を作成する地図作成手段と、をさらに備え、前記経路計画手段は、前記地図作成手段によって作成された地図及び前記画像処理手段によって検出された通行痕跡を基に前記移動体の移動経路を計画することを特徴とする。
【0010】
また、移動経路計画装置に係る第3の解決手段として、上記第1または第2の解決手段において、前記画像処理手段は、幅を有する帯状の模様を前記通行痕跡として検出することを特徴とする。
【0011】
また、移動経路計画装置に係る第4の解決手段として、上記第1または第2の解決手段において、前記画像処理手段は、幅を有し、途中で断続的になっている帯状の模様が複数、3次元空間上で平行になっている部分を前記通行痕跡として検出することを特徴とする。
【0012】
また、移動経路計画装置に係る第5の解決手段として、上記第1または第2の解決手段において、前記画像処理手段は、幅を有し、途中で断続的になっている帯状の部分を前記撮影画像中から見つけ出し、最も多数の帯状部分が共有する消失点に向かっている帯状部分を前記通行痕跡として絞り込むことを特徴とする。
【0013】
また、移動経路計画装置に係る第6の解決手段として、上記第1または第2の解決手段において、前記画像処理手段は、方向パラメータの異なる複数の空間周波数フィルタを用いて、前記撮影画像における検出対象領域をフィルタリングすることにより、前記検出対象領域についての各方向の強度分布データを生成するフィルタリング処理と、前記フィルタリング処理によって生成された前記強度分布データを基に前記検出対象領域における各画素の模様方向を特定する模様方向特定処理と、前記模様方向特定処理による前記模様方向の特定結果を基に所定の統計処理を行うことにより、前記検出対象領域において通行痕跡が存在すると推定される痕跡領域を検出する痕跡領域検出処理とを行うことを特徴とする。
【0014】
また、移動経路計画装置に係る第7の解決手段として、上記第1または第2の解決手段において、前記画像処理手段は、方向パラメータの異なる複数の空間周波数フィルタを用いて、前記撮影画像における検出対象領域をフィルタリングすることにより、前記検出対象領域についての各方向の強度分布データを生成するフィルタリング処理と、前記フィルタリング処理によって生成された前記強度分布データを基に前記検出対象領域における各画素の模様方向を特定する模様方向特定処理と、前記検出対象領域を奥行き方向に沿って複数に分割して得られる副検出対象領域ごとに、前記模様方向特定処理による前記模様方向の特定結果に基づいて所定の統計処理を行うことにより、前記副検出対象領域において通行痕跡が存在すると推定される痕跡領域を検出する痕跡領域検出処理とを行うことを特徴とする。
【0015】
また、移動経路計画装置に係る第8の解決手段として、上記第6の解決手段において、前記画像処理手段は、前記痕跡領域検出処理における前記統計処理として、前記検出対象領域における各画素のそれぞれを投票元画素として選択し、前記投票元画素を起点としてその模様方向に平行な直線上に存在する画素、または前記投票元画素を起点としてその模様方向に沿うように設定された投票対象領域に含まれる画素に対して投票を行う投票処理と、前記投票処理による投票結果を基に投票回数が最も大きい画素を消失点として特定する消失点特定処理と、前記消失点の投票元画素を有効投票画素として抽出する有効投票画素抽出処理と、前記検出対象領域において前記消失点を頂点とする領域を複数の小領域に分割し、各小領域の内、前記有効投票画素の比率が所定の閾値を超えた小領域を前記痕跡領域として特定する痕跡領域特定処理とを行うことを特徴とする。
【0016】
また、移動経路計画装置に係る第9の解決手段として、上記第7の解決手段において、前記画像処理手段は、前記痕跡領域検出処理における前記統計処理として、前記副検出対象領域における各画素のそれぞれを投票元画素として選択し、前記投票元画素を起点としてその模様方向に平行な直線上に存在する画素、または前記投票元画素を起点としてその模様方向に沿うように設定された投票対象領域に含まれる画素に対して投票を行う投票処理と、前記投票処理による投票結果を基に投票回数が最も大きい画素を消失点として特定する消失点特定処理と、前記消失点の投票元画素を有効投票画素として抽出する有効投票画素抽出処理と、前記副検出対象領域において前記消失点を頂点とする領域を複数の小領域に分割し、各小領域の内、前記有効投票画素の比率が所定の閾値を超えた小領域を前記痕跡領域として特定する痕跡領域特定処理とを行うことを特徴とする。
は、2次元ガボールフィルタであることを特徴とする。
【0017】
また、移動経路計画装置に係る第10の解決手段として、上記第9の解決手段において、前記画像処理手段は、前記投票処理では、現在着目している副検出対象領域において、以前着目していた副検出対象領域について実施された前記痕跡領域特定処理によって特定された痕跡領域の終端側に、投票の重み付けを大きくした投票重み付け領域を設定することを特徴とする。
【0018】
また、移動経路計画装置に係る第11の解決手段として、上記第10の解決手段において、前記画像処理手段は、現在着目している副検出対象領域について前記痕跡領域特定処理が終了した後に、以前着目していた副検出対象領域において、前記現在着目している副検出対象領域について実施された前記痕跡領域特定処理によって特定された痕跡領域の始端側に、投票の重み付けを大きくした投票重み付け領域を設定し、当該以前着目していた副検出対象領域について前記投票処理、消失点特定処理、有効投票画素抽出処理及び痕跡領域特定処理を再実施することで、当該以前着目していた副検出対象領域についての痕跡領域特定結果を修正することを特徴とする。
【0019】
また、移動経路計画装置に係る第12の解決手段として、上記第10または第11の解決手段において、前記画像処理手段は、前記奥行き方向に沿った位置に応じて前記投票重み付け領域の大きさ、及び/または前記投票の重み付け量を変化させることを特徴とする。
【0020】
一方、本発明では、移動体制御装置に係る第1の解決手段として、上記の第1〜第12の解決手段のいずれかを有する移動経路計画装置と、移動体の動力を発生するアクチュエータと、前記移動経路計画装置によって計画された移動経路に沿って移動体が移動するように前記アクチュエータを制御する運動制御手段とを備えることを特徴とする。
【0021】
また、移動体制御装置に係る第2の解決手段として、上記第1の解決手段において、前記移動体の位置に関する情報を取得する位置情報取得手段と、前記移動体の姿勢に関する情報を取得する姿勢情報取得手段と、前記位置情報取得手段及び前記姿勢情報取得手段によって取得された情報を基に前記移動体の現在位置及び姿勢を推定する位置・姿勢推定手段を備え、前記運動制御手段は、前記位置・姿勢推定手段によって推定された移動体の現在位置及び姿勢を基に前記移動体が前記移動経路を中心とする所定の許容範囲内を移動するように前記アクチュエータを制御することを特徴とする。
さらに、本発明では、移動体に係る解決手段として、上記の第1または第2の解決手段を有する移動体制御装置を搭載することを特徴とする。
【発明の効果】
【0022】
本発明によれば、不整地のように全体的に平坦・なだらかな地形であっても、そこある道のような通行痕跡(轍、ふみ跡など)を検出することが可能である。そして、不整地における通行痕跡は、実際に以前から何度か車両が通行していた結果生じるものであり、その延長線上は継続して通行可能なエリアになっている可能性が極めて高い。このような通行痕跡を使って計測エリア外の通行可能性が高い方向を予測することで、計測エリア外も含めて効率的な移動経路を計画することが可能となる。
【図面の簡単な説明】
【0023】
【図1】本実施形態に係る移動体制御装置を搭載した車両Cの構成概略図である。
【図2】本実施形態に係る移動体制御装置の画像処理装置2のブロック構成図である。
【図3】本実施形態における入力画像及びその入力画像データの一例である。
【図4】本実施形態に係る画像処理装置2における信号処理装置2cの動作フローチャートである。
【図5】本実施形態に係る信号処理装置2cのフィルタリング処理に関する説明図である。
【図6】本実施形態に係る信号処理装置2cの模様方向特定処理に関する説明図である。
【図7】本実施形態に係る信号処理装置2cの痕跡領域検出処理における投票処理に関する第1説明図である。
【図8】本実施形態に係る信号処理装置2cの痕跡領域検出処理における投票処理に関する第2説明図である。
【図9】本実施形態に係る信号処理装置2cの痕跡領域検出処理における投票処理に関する第3説明図である。
【図10】本実施形態に係る信号処理装置2cの痕跡領域検出処理における消失点特定処理に関する説明図である。
【図11】本実施形態に係る信号処理装置2cの痕跡領域検出処理における有効投票画素抽出処理に関する説明図である。
【図12】本実施形態に係る信号処理装置2cの痕跡領域検出処理における痕跡領域特定処理に関する説明図である。
【図13】本実施形態に係る信号処理装置2cの痕跡領域検出処理の変形例に関する第1説明図である。
【図14】本実施形態に係る信号処理装置2cの痕跡領域検出処理の変形例に関する第2説明図である。
【図15】本実施形態に係る信号処理装置2cの痕跡領域検出処理の変形例に関する第3説明図である。
【図16】本実施形態に係る地図作成装置4によって作成された地図(3次元マップ)の一例である。
【図17】本実施形態に係る経路計画装置5によって計画された移動経路の一例である。
【図18】本実施形態に係る運動制御装置9の制御によって移動する車両Cの移動範囲の一例である。
【図19】従来における移動経路計画手法の説明図である。
【発明を実施するための形態】
【0024】
以下、図面を参照して、本発明の一実施形態について説明する。
図1は、本実施形態に係る移動体制御装置を搭載した移動体の構成概略図である。本実施形態では、移動体として移動体制御装置の制御によって自律走行する車両Cを例示して説明する。なお、図1(a)は車両Cの側面図であり、図1(b)は車両Cの上面図である。また、以下では、図1に示すように、車両Cの進行方向をX軸、X軸と直交する車両Cの幅方向をY軸、XY平面(水平面)に垂直な方向(車両Cの高さ方向)をZ軸とするXYZ直交座標系を設定して説明する。
【0025】
この図1に示すように、車両Cに搭載された移動体制御装置は、撮影装置1a、1b、画像処理装置2、3次元計測装置3、地図作成装置4、経路計画装置5、GPS受信機6、ジャイロ7、位置・姿勢推定装置8、運動制御装置9及びアクチュエータ10から構成されている。なお、上記の撮影装置1a、1b、画像処理装置2、3次元計測装置3、地図作成装置4及び経路計画装置5は、本発明における移動経路計画装置を構成するものである。
【0026】
撮影装置1a、1b(撮影手段)は、例えば赤外線カメラであり、車両Cの上部左右に対となるように設置され、車両Cの進行方向を撮影して撮影画像を示す信号(画像信号)を画像処理装置2に出力する。図1に示すように、これら撮影装置1a、1bによって垂直方向(Z軸方向)の視野40°、水平方向(X軸方向)の視野60°をカバーすることが望ましい。また、上空の情報は不要であるため、車体が振動しても水平線が撮影画像の上端より飛び出ない程度に下向きに撮影装置1a、1bを設置することが望ましい(例えばY軸に対して下向きに10°傾けて設置し、水平線より下30°、上10°が視野に収まるように撮影を行う)。
【0027】
なお、図1では図示していないが、必要に応じて赤外線の投光器を車両Cの上部に設置しても良い。また、本実施形態では2つの撮影装置1a、1bを用いた場合を例示しているが、撮影装置の設置数はこれに限定されず、1つの撮影装置によって十分な撮影領域を確保可能な場合は1つだけ設置すれば良く、逆に1つでは十分な撮影領域を確保できない場合には、必要に応じて複数の撮影装置を設置しても良い。また、撮影装置1a、1bとして赤外線カメラ以外のカメラを用いても良い。
【0028】
画像処理装置2(画像処理手段)は、例えば画像処理専用のPC(Personal Computer)であり、撮影装置1a、1bから入力される画像信号(つまり撮影画像)を基に所定の画像処理を行うことにより、撮影画像に存在する通行痕跡(例えば轍など)を検出し、その通行痕跡検出結果を経路計画装置5に出力する。以下、図2を参照して、この画像処理装置2の内部構成について詳細に説明する。
【0029】
図2は、画像処理装置2のブロック構成図である。図2に示すように、画像処理装置2は、A/Dコンバータ2a、記憶装置2b、信号処理装置2c及びRAM(Random Access Memory)2dから構成されている。
【0030】
A/Dコンバータ2aは、撮影装置1a、1bから入力される画像信号をデジタルデータに変換し、入力画像(撮影画像)を示す入力画像データとして信号処理装置2cに出力する。例えば、撮影装置1a、1bの解像度をVGA(横640×縦480画素)と想定すると、上記の入力画像データは、入力画像を構成する横640×縦480個の各画素の輝度を示す輝度データの集合となる。なお、撮影装置1a、1bからデジタルデータ化された画像信号が出力される場合には、A/Dコンバータ2aを省略しても良い。
【0031】
記憶装置2bは、例えばHDD(Hard Disk Drive)であり、画像処理用プログラムPGと、方向パラメータの異なる複数(n個)の2次元ガボールフィルタ(以下、ガボールフィルタと略す)GF1〜GFnとを予め記憶しており、信号処理装置2cからの読み出し要求に応じて上記画像処理用プログラムPGやガボールフィルタGF1〜GFnを信号処理装置2cに出力する。
【0032】
ここで、画像処理用プログラムPGとは、入力画像データを基に入力画像に存在する通行痕跡を検出するための画像処理(フィルタリング処理、模様方向特定処理、強エッジ領域抽出処理、痕跡領域検出処理)を信号処理装置2cに実行させるためのプログラムである。
【0033】
また、ガボールフィルタとは、2次元ガウス関数と2次元平面上を一方向に伝播する正弦波関数とを乗じて得られる2次元ガボール関数を利用した空間周波数フィルタであり、2次元ウェーブレットフィルタの一形態である。横方向をX軸、縦方向をY軸とする2次元座標系を想定すると、2次元ガボール関数GB(x、y)は下記(1)式で表される。なお、下記(1)式において、u0は波の角周波数、σはガウス関数の標準偏差(ガウス窓の幅)を示すパラメータである。また、2次元ガボール関数GB(x、y)は、波の方向を示すパラメータ(方向パラメータ)として角周波数u0の偏角φを有している。
【0034】
【数1】
【0035】
上記のような2次元ガボール関数GB(x、y)を利用したガボールフィルタを用いて、入力画像データをフィルタリングすることにより、入力画像の周波数特性やテクスチャ(模様)方向特性を抽出することができる。具体的には、入力画像において、方向パラメータ(偏角φ)と平行なテクスチャ方向を有すると共に、周波数パラメータ(角周波数u0)に近い周波数特性を有する領域が感度良く抽出される。
【0036】
本実施形態では、上述したように、方向パラメータ(角周波数u0の偏角φ)の異なるn個のガボールフィルタGF1〜GFnを予め用意して記憶装置2bに記憶しておく。ここで、偏角φが等角度間隔で異なるようにガボールフィルタGF1〜GFnを用意する。例えば、n=72とすると、偏角φはπ/72(rad)=2.5°ずつ異なることになる。つまり、ガボールフィルタGF1の方向パラメータφ1を0(rad)と設定し、ガボールフィルタGF2の方向パラメータφ2をπ/72(rad)=2.5°と設定し、ガボールフィルタGF3の方向パラメータφ3を2π/72(rad)=5°と設定し、以下同様に、ガボールフィルタGFnの方向パラメータφnを71π/72(rad)=177.5°と設定する。
なお、nの値は入力画像の解像度や信号処理装置2cの処理能力に応じて適宜変更すれば良いが、例えばデジタル処理を考慮して8の倍数である32〜96の範囲で設定することが望ましい。
【0037】
一方、各ガボールフィルタGF1〜GFnの周波数パラメータ(角周波数u0)と減衰パラメータ(標準偏差σ)は、各フィルタ共通の所定値に設定する。ここで、周波数パラメータは、検出対象の通行痕跡の幅に応じて設定する。例えば、波の波長(周期)が視野上で1.5度〜4度となるように角周波数u0を計算して設定する。通路の撮影エリアが奥行き方向に10mから20mの領域である場合、角周波数u0が3度の視野に相当するように設定すると、撮影対象上での波の山から谷間での距離は25cm〜50cmとなり、車輪よりやや大きいサイズとなり、複数回車輪が通過した結果にできる車輪跡に近いサイズとなる。つまり、このように周波数パラメータを設定することにより、車輪跡のような轍が通路として感度良く抽出されやすくなる。また、減衰パラメータであるσは1〜0.5波長程度で設定すれば良い。
【0038】
信号処理装置2cは、例えばCPU(Central Processing Unit)であり、記憶装置2bから読み出した画像処理用プログラムPGに従って、入力画像データを基に所定の画像処理(フィルタリング処理、模様方向特定処理、強エッジ領域抽出処理、痕跡領域検出処理)を行うことにより、入力画像に存在する轍などの通行痕跡を検出し、その通行痕跡検出結果を経路計画装置5に出力するものである。
【0039】
ここで、フィルタリング処理とは、記憶装置2bからガボールフィルタGF1〜GFnを読み出し、各ガボールフィルタGF1〜GFnを用いて入力画像における検出対象領域をフィルタリングすることにより、検出対象領域についての各方向毎(φ1〜φn)の強度分布データを生成する処理を指す。また、模様方向特定処理とは、フィルタリング処理によって生成された強度分布データを基に検出対象領域における各画素のテクスチャ(模様)方向を特定する処理を指す。
【0040】
また、強エッジ領域抽出処理とは、検出対象領域に存在する強エッジ領域を抽出する処理を指す。また、痕跡領域検出処理とは、模様方向特定処理による各画素のテクスチャ方向の特定結果と、強エッジ領域抽出処理による強エッジ領域抽出結果とを基に所定の統計処理を行うことにより、検出対象領域において通行痕跡が存在すると推定される痕跡領域を検出する処理を指す。なお、これらの各種画像処理の詳細については後述する。
【0041】
RAM2dは、信号処理装置2cが各種画像処理を実行する上で必要なデータ(強度分布データやテクスチャ方向の特定結果、強エッジ領域抽出結果、統計データ等)や、上記の入力画像データを一時的に記憶するために使用される書換え可能な揮発性メモリである。
【0042】
以上が画像処理装置2の説明であり、以下では図1に戻って説明を続ける。
3次元計測装置3(3次元計測手段)は、車両Cの進行方向の3次元形状を計測するレーザ・レンジ・ファインダ(LRF)であり、車両Cの上部中央において下向きに所定角度傾けて設置されている。このようなLFRは、X軸方向の首振り(スイング)機構を有しており、図1(b)に示す走査平面SP上を走査するようにレーザ光Lを照射し、そのレーザ光Lの照射点P1、P2、・・・、Pnまでの距離データd1、d2・・・、dnと、そのスイング角度データα1、α2、・・・、αnとを、3次元形状計測結果として地図作成装置4に出力する。
【0043】
なお、市販されているLRFの計測可能範囲は、悪条件環境下において30m程度であるため、3次元計測装置3として使用するLFRも30m先の3次元形状を計測できるように設置角度や設置高さ等を設定することが望ましい。また、LFRをZ軸方向にスイングさせながらレーザ光Lを走査することで3次元形状を計測するようにしても良い。また、3次元計測装置3としてはLRF以外の装置、例えばステレオカメラ等を用いても良い。
【0044】
地図作成装置4(地図作成手段)は、例えば3次元マップ生成装置(3次元マップ作成専用のPC(Personal Computer))であり、3次元計測装置3から得られる3次元形状計測結果(距離データd1、d2・・・、dn、スイング角度データα1、α2、・・・、αn)を基に、車両Cの進行方向の地図(3次元マップ)を作成し、その3次元マップデータを経路計画装置5に出力する。なお、LFRから得られる距離データ及びスイング角度データから3次元マップを作成する手法は既に公知であるため、本実施形態では詳細な説明を省略する。
また、地図作成装置4としては、3次元マップ生成装置以外にも、荒さ・起伏マップ作成装置を利用することもでき、この場合には起伏の量を計測した地面の高さを微分した値を、上空から見た鳥瞰図的な座標にプロットして地図を作成する。
【0045】
経路計画装置5(経路計画手段)は、例えば経路計画専用のPC(Personal Computer)であり、画像処理装置2から得られる通行痕跡検出結果と、地図作成装置4から得られる3次元マップデータとを基に、車両Cの移動経路を計画し、その経路計画データを運動制御装置9に出力する。なお、この移動経路の計画手法の詳細については後述する。
【0046】
GPS受信機6(位置情報取得手段)は、車両Cの上部に設置されており、GPS(Global Positioning System:全地球測位システム)を利用して車両Cの位置に関する情報として経度、緯度、高度を取得し、その取得結果を車両位置データとして位置・姿勢推定装置8に出力する。ジャイロ7(姿勢情報取得手段)は、車両Cの所定箇所に設置された3軸ジャイロセンサであり、車両CのX軸回り(ピッチ)の回転角度及び角速度と、Y軸回り(ロール)の回転角度及び角速度と、Z軸回り(ヨー)の回転角度及び角速度とを、車両Cの姿勢情報として取得し、その取得結果を車両姿勢データとして位置・姿勢推定装置8に出力する。
【0047】
位置・姿勢推定装置8(位置・姿勢推定手段)は、例えば位置・姿勢推定専用のPC(Personal Computer)であり、GPS受信機6から得られる車両位置データと、ジャイロ7から得られる車両姿勢データと、後述するアクチュエータ10から得られる車両Cの移動量に関するフィードバックデータとを基に、車両Cの現在位置及び姿勢を推定し、その推定結果を現在位置・姿勢データとして運動制御装置9に出力する。
【0048】
運動制御装置9(運動制御手段)は、例えば車両Cの運動制御専用のPC(Personal Computer)であり、経路計画装置5から得られる経路計画データと、位置・姿勢推定装置8から得られる現在位置・姿勢データとを基に、車両Cが計画された移動経路に沿って移動するように、且つ移動経路を中心とする所定の許容範囲内を移動するようにアクチュエータ10を制御する。
【0049】
アクチュエータ10は、車両Cの車輪を駆動するためのモータ、ステアリングを駆動するためのモータ、ブレーキを駆動するためのモータや、これらモータの駆動状態を計測するセンサ(例えばステアリングの回転量を計測するポテンショメータやロータリエンコーダ、車輪の回転量を計測するロータリエンコーダ等)、及びそれらとフィードバックループを構成しつつ、運動制御装置9の制御によって各種モータに駆動電圧または電流を供給するモータアンプなどから構成されている。また、各種センサによって計測されたデータは、車両Cの移動量に関するフィードバックデータとして位置・姿勢推定装置8にフィードバックされる。
【0050】
なお、本実施形態では、画像処理装置2、地図作成装置4、経路計画装置5、位置・姿勢推定装置8及び運動制御装置9をそれぞれ独立したPCによって構成した場合を例示したが、各装置の機能を1つのPCに集約するような構成としても良い。
【0051】
次に、上記のように構成された車両C(特に移動体制御装置)の動作について詳細に説明する。
【0052】
(通行痕跡検出動作)
まず、画像処理装置2による通行痕跡検出動作について説明する。移動体制御装置の動作中において、撮影装置1a、1bは、所定のカメラレートで車両Cの進行方向を撮影し、撮影画像を示す信号(画像信号)を画像処理装置2に出力する。画像処理装置2に入力された画像信号は、A/Dコンバータ2aによって入力画像データに変換された後、信号処理装置2cに転送される。
【0053】
ここで、車両Cは川原を走行しており、撮影装置1a、1bから図3(a)に示すような轍が通行痕跡として写り込んだ撮影画像が得られたものと想定する。また、以下では、図3(b)に示すように、撮影画像の水平方向をX軸方向、垂直方向をY軸方向とし(このXY軸は図1に示すXYZ直交座標系とは無関係である)、撮影画像の座標(X、Y)=(0、0)〜(639、479)の各画素に対応する輝度データをB(0、0)〜B(639、479)と表記する。つまり、入力画像データは、これら輝度データB(0、0)〜B(639、479)の集合である。このような入力画像データは、信号処理装置2cによってRAM2dに一旦記憶される。
【0054】
図4は、信号処理装置2cの動作フローチャートである。この図4に示すように、まず、信号処理装置2cは、RAM2cから入力画像(撮影画像)における検出対象領域に相当する入力画像データを読み出すと共に、記憶装置2bからガボールフィルタGF1〜GFnを読み出し、各ガボールフィルタGF1〜GFnを用いて入力画像データをフィルタリングすることにより、各方向の強度分布データを生成する(ステップS1:フィルタリング処理)。
【0055】
本実施形態では、入力画像の全領域が上記の検出対象領域に設定されている場合を例示して説明する。つまり、RAM2dから入力画像の全領域に相当する入力画像データが読み出される。なお、入力画像から水平線を抽出可能である場合には、その水平線より下の領域(つまり地面の領域)を検出対象領域として設定しても良い。このように、確実に通行痕跡が存在すると推定される領域のみを検出対象領域とすることにより、以下で説明する各種画像処理の処理負荷を軽減でき、処理時間を短縮することができる。
【0056】
以下、図5を参照して上記ステップS1について具体的に説明する。図5(a)に示すように、信号処理装置2cは、ガボールフィルタGF1〜GFnを用いたフィルタリング処理として、入力画像において、ある画素を中心画素とする畳み込み領域に含まれる各画素の輝度データと、ガボールフィルタ、つまり2次元ガボール関数GB(x、y)との畳み込み演算を行い、その演算結果をガボールフィルタの方向パラメータφに対応する中心画素の強度として取得する。
【0057】
つまり、信号処理装置2cは、まず、ある画素を中心画素とする畳み込み領域に含まれる各画素の輝度データとガボールフィルタGF1との畳み込み演算を行い、その演算結果をガボールフィルタGF1の方向パラメータφ1に対応する中心画素の強度として取得する。ここで、各ガボールフィルタGF1〜GFnを、実数部のフィルタと虚数部のフィルタとに分けて作成しておくことにより、実数部のフィルタを用いて得られた強度と、虚数部のフィルタを用いて得られた強度とをそれぞれ2乗して加算し、その加算値の平方根を求めることで最終的な強度を取得することができる。
【0058】
続いて、信号処理装置2cは、ガボールフィルタGF2に切り替えて同様に畳み込み演算を行い、その演算結果をガボールフィルタGF2の方向パラメータφ2に対応する中心画素の強度として取得する。以下、同様に、信号処理装置2cは、ガボールフィルタを順次切り替えながら畳み込み演算を行うことにより、その演算結果を各ガボールフィルタGF1〜GFnの方向パラメータφ1〜φnに対応する中心画素の強度として取得する。
【0059】
信号処理装置2cは、上記のようなフィルタリング処理を、入力画像の座標(0、0)〜(639、479)の各画素のそれぞれについて行うことにより、図5(b)に示すような各方向の強度分布データを生成してRAM2dに記憶する。なお、図5(b)では、座標(0、0)〜(639、479)の各画素のそれぞれを中心画素として得られた強度データをI(0、0)〜I(639、479)として表記している。
【0060】
なお、図5(a)からわかるように、入力画像の端部に近い画素を中心画素とする場合、畳み込み領域が入力画像からはみ出てしまうため、この場合には予め設定した輝度データ(例えば白データ等)を入力画像からはみ出ている畳み込み領域の輝度データとして使用すれば良い。また、畳み込み領域の大きさは、検出対象の通行痕跡の大きさに応じて適宜変更すれば良いが、例えば幅25cm〜50cmの車輪跡に相当する轍を検出対象とする場合には、その幅に相当する画素数(32画素)以上の大きさ、例えば50×50画素の大きさとすることが望ましい。
【0061】
次に、上記のようなステップS1が終了すると、信号処理装置2cは、RAM2dから各方向の強度分布データを読み出し、それら強度分布データを基に入力画像における各画素のテクスチャ(模様)方向を特定する(ステップS2:模様方向特定処理)。具体的には、例えば座標(0、0)の画素に着目した場合、各方向の強度分布データの中から強度データI(0、0)が最も大きいものを探索し、その最大の強度データI(0、0)を含む強度分布データに対応する方向を、座標(0、0)の画素のテクスチャ方向として特定する。例えば、方向φ3に対応する強度分布データに最大の強度データI(0、0)が含まれている場合、方向φ3が座標(0、0)の画素のテクスチャ方向となる。
【0062】
信号処理装置2cは、上記のようなテクスチャ方向の特定処理を、座標(0、0)〜(639、479)の各画素について行うことにより、各画素のテクスチャ方向を特定し、その特定結果をRAM2dに記憶する。図6(a)は各画素のテクスチャ方向の特定結果であり、図6(b)は入力画像とテクスチャ方向の特定結果とを重ねて表示したものである。なお、この図6では、説明の便宜上、テクスチャ方向の特定結果を、テクスチャ方向を指し示すベクトル線で表しているが、実際にはテクスチャ方向の特定結果は、その方向を表す値(例えばテクスチャ方向がφ1であればφ1の値)によって構成されている。
【0063】
不整地上に存在する轍などの通行痕跡は、車両が複数回その場所を走行することにより、地面や草むらが踏み起こされたり、踏み固められたりした結果残るものであるため、図3に示すように、その痕跡は入力画像上において不連続且つ不明瞭な帯状やスジ状の疎らな領域として観察される。そこで、上述した入力画像の波の成分を抽出する機能を有するステップS1及びステップS2を実施することより、通行痕跡を形成する模様の間隔は不定でなだらかではあるが、通行痕跡の進行方向と平行な波の成分として捕らえることができる。
【0064】
次に、上記のようなステップS2が終了すると、信号処理装置2cは、入力画像データを基に所定のエッジ検出処理を行うことにより、入力画像に存在する強エッジ領域を抽出し、その抽出結果をRAM2dに記憶する(ステップS3:強エッジ領域検出処理)。ここで、エッジ検出処理としては公知の技術を採用することができる。例えば、X軸方向のソベルフィルタとY軸方向のソベルフィルタを適用して、それらの強度の2乗和が一定の閾値を超えた画素を探索し、その閾値を超えた画素から一定距離以内の領域を強エッジ領域として抽出する。
【0065】
次に、上記のようなステップS3が終了すると、信号処理装置2cは、模様方向特定処理による各画素のテクスチャ方向の特定結果と、強エッジ領域抽出処理による強エッジ領域抽出結果とを基に所定の統計処理を行うことにより、入力画像において通行痕跡が存在すると推定される痕跡領域を検出する(ステップS4:痕跡領域検出処理)。具体的には、信号処理装置2cは、ステップS4における統計処理として、投票処理(ステップS4a)、消失点特定処理(ステップS4b)、有効投票画素抽出処理(ステップS4c)及び痕跡領域特定処理(ステップS4d)を行う。以下、それぞれの処理について詳細に説明する。
【0066】
<投票処理>
まず、信号処理装置2cは、ステップS4aの投票処理として、入力画像における各画素のそれぞれを投票元画素として順次選択し、当該投票元画素を起点としそのテクスチャ方向に平行な直線上に存在する画素に対して投票を行う。具体的には、信号処理装置2cは、まず、図7に示すように、RAM2dのメモリ空間上に、投票回数を集計するための投票集計領域を入力画像と同サイズ(つまり、640×480画素数分の投票回数を集計可能なサイズ)で確保する。
【0067】
そして、信号処理装置2cは、図8(a)に示すように、ある画素を投票元画素として選択した場合、模様方向特定処理による各画素のテクスチャ方向の特定結果から投票元画素のテクスチャ方向を把握し、投票元画素を起点としそのテクスチャ方向に平行な直線上に存在する画素(投票対象画素)に対して投票を行い、その投票結果を図8(b)に示すようにRAM2d上の投票集計領域に反映する。つまり、図8(b)からわかるように、投票集計領域において投票対象画素に対応する記憶領域に投票値「1」が加算される。
【0068】
信号処理装置2cは、上記のような投票処理を各画素について行い、その投票結果を順次RAM2d上の投票集計領域に反映する。ここで、信号処理装置2cは、上記のような投票処理を行う際、強エッジ領域抽出処理によって抽出された強エッジ領域の周辺(たとえば2画素以内)に含まれる画素を投票元画素から除外する。この理由については後述する。
【0069】
なお、上述の投票処理では、投票元画素を起点としそのテクスチャ方向に平行な直線上に存在する投票対象画素に対して投票を行う場合を説明したが、これに限らず、投票元画素を起点としてそのテクスチャ方向に沿うように設定された投票対象領域に含まれる画素(投票対象画素)に対して投票を行うようにしても良い。この投票対象領域は、例えば図9(a)に示すように、テクスチャ方向に平行な帯状(長方形状)のものでも良いし、また、図9(b)に示すように、円弧状あるいは三角形状のものでも良い。
【0070】
ここで、図9(a)に示すような帯状の投票対象領域を設定する場合には、上述したテクスチャ方向に平行な直線上に存在する投票対象画素に対して投票を行う場合と同様に、投票対象領域に含まれる投票対象画素のそれぞれに対して平等に投票すれば良い。つまり、RAM2d上の投票集計領域において、各投票対象画素に対応する記憶領域に平等に投票値「1」を加算する。
【0071】
一方、図9(b)に示すような円弧状あるいは三角形状の投票対象領域を設定する場合には、投票元画素と投票対象画素との間の距離に応じて投票値を変更することが望ましい。具体的には、投票元画素から遠い距離に位置する投票対象画素の投票値を小さくする(例えば、投票値=1/投票元画素からの距離)。つまり、投票元画素に近い位置の投票対象画素の投票値は「1」または「1」に近い値となるが、投票元画素から遠い位置の投票対象画素の投票値は「0.5」等の小数点以下の値となる。さらには、投票対象領域において同じ距離に位置する投票対象画素の総投票値が「1」となるように各投票対象画素の投票値を決定することが望ましい。つまり、例えば同じ距離に位置する投票対象画素が4つ存在すると仮定すると、それら4つの各投票対象画素に対する投票値をそれぞれ「0.25」とする。
【0072】
このように円弧状あるいは三角形状の投票対象領域を設定した場合に、投票元画素と投票対象画素との間の距離に応じて投票値を変更する理由は、投票元画素から遠い位置にある投票対象画素は、他の投票対象領域と重なり合う確率が高く、近い位置にある投票対象画素と比べて投票されやすい傾向にあり、後述する消失点を正確に特定することが困難となるためである。また、円弧状あるいは三角形状の投票対象領域を設定した場合、その中心角度(投票元画素を起点とする角度)は、ガボールフィルタGF1〜GFnの方向パラメータφの変化分(つまりπ/72(rad)=2.5°)と同一にすることが望ましい。
【0073】
<消失点特定処理>
続いて、信号処理装置2cは、ステップS4bの消失点特定処理として、図10に示すように、投票処理による投票結果(つまり、RAM40上の投票集計領域に集計された投票回数)を基に、投票回数が最も大きい画素を入力画像における消失点として特定する。轍などの通行痕跡の波模様は車両Cから見ると完全に平行ではないが同じ消失点を共有していると推測される。このため、上記のような投票処理によって消失点と推測される位置(画素)に投票を行い、統計的に通行痕跡と平行している波模様が多い検出結果を活用することにより、消失点を特定することができる。
【0074】
ここで、入力画像において通行痕跡以外に、例えば木や電柱、看板などのコントラストの高い物体が写り込んでいた場合、それらの模様は上記の投票回数の集計に大きな影響を及ぼすことになる。そこで、上述したように、投票処理を行う際には、強エッジ領域抽出処理によって抽出された強エッジ領域に含まれる画素を投票元画素から除外することにより、木や電柱、看板などのコントラストの高い物体の延長線上が消失点として特定されることを防止することができる。
【0075】
<有効投票画素抽出処理>
続いて、信号処理装置2cは、ステップS4cの有効投票画素抽出処理として、消失点の投票元画素を有効投票画素として抽出する。つまり、消失点を向いているテクスチャ方向を有する画素が有効投票画素として抽出されることになる。図11に、有効投票画素の抽出結果を示す。図11において白領域が有効投票画素の集合領域である。
【0076】
<痕跡領域特定処理>
続いて、信号処理装置2cは、ステップS4dの痕跡領域特定処理として、図12に示すように、入力画像において消失点を頂点とする領域(この領域は円弧状でも良いし、三角形状でも良い)を複数の小領域に分割し、各小領域の内、有効投票画素の比率が所定の閾値を超えた小領域を通行痕跡が存在すると推定される痕跡領域として特定する。ここで、有効投票画素の比率は、小領域と有効投票画素との面積比率、つまり、小領域に含まれる有効投票画素数を小領域に含まれる全画素数で除算することで求めることができる。
【0077】
入力画像において轍などの通行痕跡が写り込んでいる領域では、周囲と比べて高い比率で消失点を向いているテクスチャを含んでいると推測される。そこで、上記のような有効投票画素抽出処理及び痕跡領域特定処理を行うことにより、入力画像において通行痕跡が存在する痕跡領域と他の領域とを判別することができる。
【0078】
なお、痕跡領域は轍などの通行痕跡に沿って検出されるものであるので、隣接する痕跡領域は、ある規則性を持って連続的または断続的に連なっているはずである。そこで、入力画像において、ある規則性を持った痕跡領域の集まりから孤立している痕跡領域はノイズとして捉え、そのような孤立領域は痕跡領域から除去することにより、通行痕跡の誤検出を防止することができる。
【0079】
ところで、上記説明は、図3に示すように、通行痕跡が水平線に向かって一直線に延びているような入力画像が得られた場合を想定したものである。しかしながら、図13に示すように、入力画像中の通行痕跡がカーブしているような場合には、入力画像の奥行き毎に消失点が水平方向に変化することになる。このような場合、上述したように入力画像の全領域(検出対象領域の全領域)を一度に処理すると、正確な消失点を得ることができなくなる。
【0080】
そこで、このような問題に対処するために、上記のステップS4の処理(投票処理、消失点特定処理、有効投票画素抽出処理及び痕跡領域特定処理)を、図13に示すように、検出対象領域(ここでは入力画像の全領域)を奥行き方向に沿って複数に分割して得られる副検出対象領域ごとに行うようにしても良い。このように、副検出対象領域毎に消失点を求め、その対応する奥行きごとに消失点に適合する有効投票画素を求めることで、それぞれの奥行きに対応した消失点と痕跡領域の切り出しを行うことができる。
【0081】
ここで、2次元ウェーブレットフィルタを用いた場合、入力画像上で強い方向性を有する領域が轍などの痕跡領域として検出されるため、轍の外に生えている草や樹木、人工物の平行な特徴を誤って検出してしまう可能性がある。そこで、上記のように、奥行き方向に沿って複数に分割して得られる副検出対象領域ごとに痕跡領域の特定を行う場合、以下に説明する処理を加えることで検出精度の向上を図ることができる。
【0082】
実験の結果、轍検出(痕跡領域検出)の精度は各地点までの奥行き方向の距離に依存し、手前ほど精度が高いことが判明した。また、検出済みの轍(痕跡領域)と連続した領域はやはり轍(痕跡領域)である可能性が大きいと考えられる。そこで、入力画像を手前から奥へ数段階に分割し、最も手前側の副検出対象領域に存在する轍(痕跡領域)を最初に検出して、それを基準に以遠の轍(痕跡領域)を検出することとする。
【0083】
図14に示すように、まず、奥行き方向の最も手前側の副検出対象領域(ここでは第1副検出対象領域W1とする)について、上記実施形態で説明したステップS4の処理(投票処理、消失点特定処理、有効投票画素抽出処理及び痕跡領域特定処理)を行うことで第1副検出対象領域W1における痕跡領域特定結果を得る。なお、図14では、痕跡領域の特定結果を第1副検出対象領域W1の消失点に向かう連続した直線として表している。
【0084】
続いて、次段の副検出対象領域(ここでは第2副検出対象領域W2とする)について上記ステップS4の処理を行うが、この時の投票処理では、現在着目している副検出対象領域(つまり第2副検出対象領域W2)において、図14に示すように、以前着目していた副検出対象領域(つまり第1副検出対象領域W1)について実施された痕跡領域特定処理によって特定された痕跡領域の終端側に、投票の重み付けを大きくした投票重み付け領域Waを設定する。このような投票重み付け領域Waは、痕跡領域の特定結果を表す各直線上のそれぞれの終端部を中心に設定した所定サイズの矩形領域から構成されている。この矩形領域の1つ当たりのサイズは、縦:0.08×画素高さ、横:0.03×画素幅、程度とする。
【0085】
このように投票重み付け領域Waを設定した第2副検出対象領域W2について上記ステップS4の投票処理を行う場合、投票重み付け領域Waに存在する投票対象画素に対する投票には通常の3倍程度の重み付けを行う。例えば、1段目の第1副検出対象領域W1の投票処理において投票対象画素に対する投票値を「1」とした場合、第2副検出対象領域W2の投票重み付け領域Waに存在する投票対象画素に対する投票値は「3」とする。
【0086】
このような第2副検出対象領域W2についてステップS4の投票処理、消失点特定処理、有効投票画素抽出処理及び痕跡領域特定処理を行うことにより、前段の第1副検出対象領域W1について特定された痕跡領域の終端が、次段の第2副検出対象領域W2の痕跡領域の起点となりやすくなる。言い換えれば、前段の第1副検出対象領域W1の痕跡領域と次段の第2副検出対象領域W2の痕跡領域とが連続した痕跡領域として検出されやすくなり、入力画像中に轍などと平行に生えた草や樹木、人工物等が存在する場合であっても、精度良く轍などの痕跡領域を検出することができる。
【0087】
上記のように第2副検出対象領域W2について痕跡領域の特定結果を得た後は、次段(3段目)の副検出対象領域について第2副対象領域W2と同様の処理を行う。なお、3段目の副検出対象領域では、投票重み付け領域を構成する矩形領域の1つ当たりのサイズを、2段目の第2副検出対象領域W2の半分、つまり、縦:0.04×画素高さ、横:0.015×画素幅、程度とする。
【0088】
また、3段目の副検出対象領域について上記ステップS4の投票処理を行う場合、投票重み付け領域に存在する投票対象画素に対する投票には通常の5倍程度の重み付けを行う。例えば、1段目の第1副検出対象領域W1の投票処理において投票対象画素に対する投票値を「1」とした場合、3段目の副検出対象領域の投票重み付け領域に存在する投票対象画素に対する投票値は「5」とする。このように奥行き方向に沿った位置に応じて投票重み付け領域の大きさ及び投票の重み付け量を変化させることにより、より精度良く痕跡領域の連続性を確保することができる。
【0089】
以降、同様な処理を奥行き方向の最も遠い副検出対象領域まで行うことにより、各段の副検出対象領域で特定された痕跡領域が連続した痕跡領域として精度良く検出され、轍などの車両の走行痕を正確に検出することが可能となる。なお、4段目以降の副検出対象領域では、投票重み付け領域を構成する矩形領域の1つ当たりのサイズ及び投票の重み付け量を3段目の副検出対象領域と同一にすることが好ましい。
【0090】
図15(a)は、投票重み付け領域を使用しない場合における痕跡領域の特定結果を示すものである。この図15(a)に示すように、投票重み付け領域を使用しない場合、本来、痕跡領域とすべき領域とは異なる領域を痕跡領域として特定してしまっていることがわかる。また、図15(b)は、投票重み付け領域を使用した場合における痕跡領域の特定結果を示すものである。この図15(b)に示すように、投票重み付け領域を使用した場合、本来、痕跡領域とすべき領域を正しく痕跡領域として特定していることがわかる。
【0091】
なお、例えば、現在着目している副検出対象領域(例えば第2副検出対象領域W2)について痕跡領域特定処理が終了した後に、以前着目していた副検出対象領域(第1副検出対象領域W1)において、第2副検出対象領域W2について実施された痕跡領域特定処理によって特定された痕跡領域の始端側に、投票の重み付けを大きくした投票重み付け領域を設定し、当該第1副検出対象領域W1についてステップS4の投票処理、消失点特定処理、有効投票画素抽出処理及び痕跡領域特定処理を再実施することで、当該第1副検出対象領域W1についての痕跡領域特定結果を修正することも可能である。
【0092】
以上のように、画像処理装置2における信号処理装置2cは、上述した画像処理を実施することによって、入力画像において通行痕跡が存在すると推定される痕跡領域を検出し、その検出結果を通行痕跡検出結果として経路計画装置5に出力する。なお、上述した画像処理を、カメラレートで撮影装置1a、1bから送られる撮影画像毎に実施する必要はなく、毎秒2枚から10枚程度の撮影画像について実施すれば良い。
【0093】
(3次元マップ作成動作)
続いて、地図作成装置4による3次元マップ作成動作について説明する。移動体制御装置の動作中において、3次元計測装置3(LRF)は、図1(b)に示す走査平面SP上を走査するようにレーザ光Lを照射し、そのレーザ光Lの照射点P1、P2、・・・、Pnまでの距離データd1、d2・・・、dnと、そのスイング角度データα1、α2、・・・、αnとを、3次元形状計測結果として地図作成装置4に出力する。ここで、図16(a)に示すように、車両Cを走行させながらレーザ光Lを走査することで3次元形状を計測しても良いし、LFRをZ軸方向にスイングさせながらレーザ光Lを走査することで3次元形状を計測しても良い。
【0094】
地図作成装置4は、3次元計測装置3から得られる3次元形状計測結果(距離データd1、d2・・・、dn、スイング角度データα1、α2、・・・、αn)を基に、図16(b)に示すように、車両Cの進行方向の3次元マップを作成し、その3次元マップデータを経路計画装置5に出力する。なお、LFRから得られる距離データ及びスイング角度データから3次元マップを作成する手法は既に公知であるため、本実施形態では詳細な説明を省略する。
【0095】
(経路計画動作)
続いて、経路計画装置5による経路計画動作について説明する。経路計画装置5は、画像処理装置2から得られる通行痕跡検出結果と、地図作成装置4から得られる3次元マップデータとを基に、轍(通行痕跡)に沿った方向、且つ轍から一定以内の距離を保つ範囲の中という条件の下、周囲の3次元形状から最適な移動経路を計画する。
【0096】
より具体的には、経路計画装置5は、移動経路上の起伏、および移動経路上の各地点でのステアリング量など走行の安定性を低下させる要因をその度合いに応じたコスト値に変換し、速度・回転半径などの車体性能の制約条件を満たしつつ、そのコストが最小となるように移動経路を計画する。もしくは、移動経路上の起伏度合いとステアリングからそこで許容されている最大速度を求め、その移動経路上での最大速度に近くなるような加減速を行うことをシミュレーションし、その到達時間が最短となる移動経路を計画しても良い。
【0097】
また、作成された3次元マップから周囲の3次元形状の変化を監視し、大きな変化がない場合には、近傍は再計画を行わず、遠方(たとえば計画を行う範囲の1/2以上遠方)のみを再計画するようにしても良い。これにより、近傍の軌道が再計画により細々変動したり軌道が振動したりして移動経路が不安定になることを防ぐことができる。一方、3次元マップや轍などの通行痕跡検出結果が大きく変化した場合には、全経路での再計画を行うことが望ましい。さらに、轍に沿った範囲で安定に走行できる条件での計画ができなかった場合には、モードを轍を考慮しないモード(つまり画像処理装置2から得られる通行痕跡検出結果を無視するモード)に切り替え、従来技術と同様に、3次元マップのみを用いて最適な移動経路を計画することが望ましい。
以上のような手法により、経路計画装置5は、図17に示すような車両Cの移動経路を計画し、その経路計画データを運動制御装置9に出力する。
【0098】
(車両Cの現在位置及び姿勢の推定動作)
続いて、位置・姿勢推定装置8による車両Cの現在位置及び姿勢の推定動作について説明する。移動体制御装置の動作中において、GPS受信機6は、車両Cの位置に関する情報として経度、緯度、高度を取得し、その取得結果を車両位置データとして位置・姿勢推定装置8に出力する。また、ジャイロ7は、車両CのX軸回り(ピッチ)の回転角度及び角速度と、Y軸回り(ロール)の回転角度及び角速度と、Z軸回り(ヨー)の回転角度及び角速度とを、車両Cの姿勢情報として取得し、その取得結果を車両姿勢データとして位置・姿勢推定装置8に出力する。さらに、アクチュエータ10の各種センサによって計測されたデータは、車両Cの移動量に関するフィードバックデータとして位置・姿勢推定装置8にフィードバックされる。
【0099】
位置・姿勢推定装置8は、GPS受信機6から得られる車両位置データと、ジャイロ7から得られる車両姿勢データと、アクチュエータ10から得られる車両Cの移動量に関するフィードバックデータとに加えて、GPS受信機6で受信可能な衛星数から求まる車両位置の計測誤差、及び不整地で事前に測定した車輪の滑りから求まる計測誤差に基づいて、尤もらしい車両Cの現在位置及び姿勢を推定し、その推定結果を現在位置・姿勢データとして運動制御装置9に出力する。なお、この推定にはカルマンフィルタやパーティクルフィルタを用いることで、より高精度に車両Cの現在位置及び姿勢を求めることが可能になる。
【0100】
(車両Cの運動制御動作)
続いて、運動制御装置9による車両Cの運動制御動作について説明する。運動制御装置9は、経路計画装置5から得られる経路計画データと、位置・姿勢推定装置8から得られる現在位置・姿勢データとを基に、車両Cが計画された移動経路に沿って移動するように、且つ移動経路を中心とする所定の許容範囲内を移動するようにアクチュエータ10を制御する。
【0101】
具体的には、まず、計画された移動経路及び速度となるように、車両Cの現在位置及び姿勢を基に計画された移動経路に対するズレ量を加算し、所定の許容範囲内で移動経路に沿うように車両Cの目標旋回半径を決定する(移動経路に十分近ければ、移動経路の旋回半径を目標旋回半径に決定する)。そして、目標旋回半径の決定後、予め作成しておいたステアリング角・速度と旋回半径とを対応付けるテーブルに基づいて、現在速度及び目標旋回半径から実施すべきステアリング角を決定する。運動制御装置9は、上記のように決定したステアリング角となるようにアクチュエータ10に対してステアリングの回転量を指示する。
【0102】
一方、現在の旋回半径、及び近傍(10m程度)の移動経路の旋回半径の最も厳しい(半径の小さい)値から車両Cが安全に旋回するための最大速度を求め(車両Cの物理モデルと砂地などの摩擦係数を用いてシミュレーションを行って事前に求めておく)、最大速度と計画された速度との小さい方を車両Cの目標速度とする。運動制御装置9は、上記のように決定した目標速度と現在の速度とを基に、PID制御などを用いて制御値を決定し、アクチュエータ10の車輪を駆動するためのモータを制御する。
なお、移動経路に対するズレ量が大きくなった場合には、経路計画装置5に対して移動経路の再計画を指示し、以前の移動経路に依存せずに、現在位置からスムーズな移動経路を計画することが望ましい。
【0103】
以上説明したような運動制御装置9による運動制御動作によって、図18に示すように、車両Cは計画された移動経路に沿って移動するように、且つ移動経路を中心とする所定の許容範囲内を移動するように制御されることになる。
【0104】
以上のように、本実施形態によれば、不整地のように全体的に平坦・なだらかな地形であっても、そこある道のような通行痕跡(轍、ふみ跡など)を検出することが可能である。そして、不整地における通行痕跡は、実際に以前から何度か車両が通行していた結果生じるものであり、その延長線上は継続して通行可能なエリアになっている可能性が極めて高い。このような通行痕跡を使って計測エリア外の通行可能性が高い方向を予測することで、計測エリア外も含めて効率的な移動経路を計画することが可能となる。
【0105】
なお、本発明は上記実施形態に限定されず、以下のような変形例が考えられる。
(1)上記実施形態では、各ガボールフィルタGF1〜GFnの周波数パラメータ(角周波数u0)を固定値とする場合を例示した。これは、通路の撮影エリアが奥行き方向に10mから20mの領域である場合、角周波数u0が視野中の3度に相当するように設定すると、撮影対象上での波の山から谷間での距離は25cm〜50cmとなり、車輪よりやや大きいサイズとなり、複数回車輪が通過した結果にできる車輪跡に近いサイズとなるためである。しかしながら、図3の入力画像からわかるように、轍などの通行痕跡の幅は、奥行き方向の位置によって変化する。そこで、入力画像における奥行き方向の位置に応じて、各ガボールフィルタGF1〜GFnの周波数パラメータを変更することが望ましい。
【0106】
具体的には、記憶装置2bに、入力画像における奥行き方向の位置に応じた周波数パラメータを有すると共に方向パラメータの異なるガボールフィルタを予め記憶しておき、信号処理装置2cに、記憶装置2bから検出対象領域におけるフィルタリング対象位置(つまり図5中の中心画素の位置)に対応する周波数パラメータを有するガボールフィルタを読み出してフィルタリング処理する機能を持たせれば良い。
【0107】
(2)上記実施形態では、空間周波数フィルタとして2次元ガボールフィルタを用いた場合を例示したが、これに限らず、その他の2次元ウェーブレットフィルタや2次元窓付きフーリエ変換を利用したフィルタなどを空間周波数フィルタとして用いても良い。
【0108】
(3)また、上記実施形態では、入力画像に存在する通行痕跡を検出する画像処理手法として、空間周波数フィルタを用いた場合を例示して説明したが、これに限定されず、通行痕跡を検出可能な画像処理手法であれば他の手法を採用しても良い。すなわち、撮影画像の中から幅を有する帯状の模様を通行痕跡として検出する手法、または、幅を有し、途中で断続的になっている帯状の模様が複数、3次元空間上で平行になっている部分を通行痕跡として検出する手法、或いは、幅を有し、途中で断続的になっている帯状の部分を撮影画像中から見つけ出し、最も多数の帯状部分が共有する消失点に向かっている帯状部分を通行痕跡として絞り込む手法であれば、他の手法を用いても良い。
【0109】
(4)上記実施形態では、移動体制御装置を搭載した移動体として車両Cを例示して説明したが、移動体はこれに限らず、ロボットなどの2足歩行をする移動体であっても本発明における移動体制御装置を適用することが可能である。
【符号の説明】
【0110】
C…車両、1a、1b…撮影装置、2…画像処理装置、3…3次元計測装置、4…3次元マップ作成装置、5…経路計画装置、6…GPS受信機、7…ジャイロ、8…位置・姿勢推定装置、9…運動制御装置、10…アクチュエータ、2a…A/Dコンバータ、2b…記憶装置、2c…信号処理装置、2d…RAM(Random Access Memory)、PG…通路検出プログラム、GF1〜GFn…ガボールフィルタ
【特許請求の範囲】
【請求項1】
移動体の進行方向を撮影する撮影手段と、
前記撮影手段から得られる撮影画像を基に所定の画像処理を行うことにより、前記撮影画像に存在する通行痕跡を検出する画像処理手段と、
前記画像処理手段によって検出された通行痕跡を基に前記移動体の移動経路を計画する経路計画手段と、
を備えることを特徴とする移動経路計画装置。
【請求項2】
移動体の進行方向の3次元形状を計測する3次元計測手段と、
前記3次元計測手段から得られる3次元形状の計測結果を基に地図を作成する地図作成手段と、をさらに備え、
前記経路計画手段は、前記地図作成手段によって作成された地図及び前記画像処理手段によって検出された通行痕跡を基に前記移動体の移動経路を計画することを特徴とする請求項1記載の移動経路計画装置。
【請求項3】
前記画像処理手段は、幅を有する帯状の模様を前記通行痕跡として検出することを特徴とする請求項1または2に記載の移動経路計画装置。
【請求項4】
前記画像処理手段は、幅を有し、途中で断続的になっている帯状の模様が複数、3次元空間上で平行になっている部分を前記通行痕跡として検出することを特徴とする請求項1または2に記載の移動経路計画装置。
【請求項5】
前記画像処理手段は、幅を有し、途中で断続的になっている帯状の部分を前記撮影画像中から見つけ出し、最も多数の帯状部分が共有する消失点に向かっている帯状部分を前記通行痕跡として絞り込むことを特徴とする請求項1または2に記載の移動経路計画装置。
【請求項6】
前記画像処理手段は、
方向パラメータの異なる複数の空間周波数フィルタを用いて、前記撮影画像における検出対象領域をフィルタリングすることにより、前記検出対象領域についての各方向の強度分布データを生成するフィルタリング処理と、
前記フィルタリング処理によって生成された前記強度分布データを基に前記検出対象領域における各画素の模様方向を特定する模様方向特定処理と、
前記模様方向特定処理による前記模様方向の特定結果を基に所定の統計処理を行うことにより、前記検出対象領域において通行痕跡が存在すると推定される痕跡領域を検出する痕跡領域検出処理と、を行う
ことを特徴とする請求項1または2に記載の移動経路計画装置。
【請求項7】
前記画像処理手段は、
方向パラメータの異なる複数の空間周波数フィルタを用いて、前記撮影画像における検出対象領域をフィルタリングすることにより、前記検出対象領域についての各方向の強度分布データを生成するフィルタリング処理と、
前記フィルタリング処理によって生成された前記強度分布データを基に前記検出対象領域における各画素の模様方向を特定する模様方向特定処理と、
前記検出対象領域を奥行き方向に沿って複数に分割して得られる副検出対象領域ごとに、前記模様方向特定処理による前記模様方向の特定結果に基づいて所定の統計処理を行うことにより、前記副検出対象領域において通行痕跡が存在すると推定される痕跡領域を検出する痕跡領域検出処理と、を行う
ことを特徴とする請求項1または2に記載の移動経路計画装置。
【請求項8】
前記画像処理手段は、前記痕跡領域検出処理における前記統計処理として、
前記検出対象領域における各画素のそれぞれを投票元画素として選択し、前記投票元画素を起点としてその模様方向に平行な直線上に存在する画素、または前記投票元画素を起点としてその模様方向に沿うように設定された投票対象領域に含まれる画素に対して投票を行う投票処理と、
前記投票処理による投票結果を基に投票回数が最も大きい画素を消失点として特定する消失点特定処理と、
前記消失点の投票元画素を有効投票画素として抽出する有効投票画素抽出処理と、
前記検出対象領域において前記消失点を頂点とする領域を複数の小領域に分割し、各小領域の内、前記有効投票画素の比率が所定の閾値を超えた小領域を前記痕跡領域として特定する痕跡領域特定処理と、を行う
ことを特徴とする請求項6記載の移動経路計画装置。
【請求項9】
前記画像処理手段は、前記痕跡領域検出処理における前記統計処理として、
前記副検出対象領域における各画素のそれぞれを投票元画素として選択し、前記投票元画素を起点としてその模様方向に平行な直線上に存在する画素、または前記投票元画素を起点としてその模様方向に沿うように設定された投票対象領域に含まれる画素に対して投票を行う投票処理と、
前記投票処理による投票結果を基に投票回数が最も大きい画素を消失点として特定する消失点特定処理と、
前記消失点の投票元画素を有効投票画素として抽出する有効投票画素抽出処理と、
前記副検出対象領域において前記消失点を頂点とする領域を複数の小領域に分割し、各小領域の内、前記有効投票画素の比率が所定の閾値を超えた小領域を前記痕跡領域として特定する痕跡領域特定処理と、を行う
ことを特徴とする請求項7記載の移動経路計画装置。
【請求項10】
請求項1〜9のいずれか一項に記載の移動経路計画装置と、
移動体の動力を発生するアクチュエータと、
前記移動経路計画装置によって計画された移動経路に沿って移動体が移動するように前記アクチュエータを制御する運動制御手段と、
を備えることを特徴とする移動体制御装置。
【請求項11】
前記移動体の位置に関する情報を取得する位置情報取得手段と、
前記移動体の姿勢に関する情報を取得する姿勢情報取得手段と、
前記位置情報取得手段及び前記姿勢情報取得手段によって取得された情報を基に前記移動体の現在位置及び姿勢を推定する位置・姿勢推定手段を備え、
前記運動制御手段は、前記位置・姿勢推定手段によって推定された移動体の現在位置及び姿勢を基に前記移動体が前記移動経路を中心とする所定の許容範囲内を移動するように前記アクチュエータを制御することを特徴とする請求項10記載の移動体制御装置。
【請求項12】
請求項10または11に記載の移動体制御装置を搭載することを特徴とする移動体。
【請求項1】
移動体の進行方向を撮影する撮影手段と、
前記撮影手段から得られる撮影画像を基に所定の画像処理を行うことにより、前記撮影画像に存在する通行痕跡を検出する画像処理手段と、
前記画像処理手段によって検出された通行痕跡を基に前記移動体の移動経路を計画する経路計画手段と、
を備えることを特徴とする移動経路計画装置。
【請求項2】
移動体の進行方向の3次元形状を計測する3次元計測手段と、
前記3次元計測手段から得られる3次元形状の計測結果を基に地図を作成する地図作成手段と、をさらに備え、
前記経路計画手段は、前記地図作成手段によって作成された地図及び前記画像処理手段によって検出された通行痕跡を基に前記移動体の移動経路を計画することを特徴とする請求項1記載の移動経路計画装置。
【請求項3】
前記画像処理手段は、幅を有する帯状の模様を前記通行痕跡として検出することを特徴とする請求項1または2に記載の移動経路計画装置。
【請求項4】
前記画像処理手段は、幅を有し、途中で断続的になっている帯状の模様が複数、3次元空間上で平行になっている部分を前記通行痕跡として検出することを特徴とする請求項1または2に記載の移動経路計画装置。
【請求項5】
前記画像処理手段は、幅を有し、途中で断続的になっている帯状の部分を前記撮影画像中から見つけ出し、最も多数の帯状部分が共有する消失点に向かっている帯状部分を前記通行痕跡として絞り込むことを特徴とする請求項1または2に記載の移動経路計画装置。
【請求項6】
前記画像処理手段は、
方向パラメータの異なる複数の空間周波数フィルタを用いて、前記撮影画像における検出対象領域をフィルタリングすることにより、前記検出対象領域についての各方向の強度分布データを生成するフィルタリング処理と、
前記フィルタリング処理によって生成された前記強度分布データを基に前記検出対象領域における各画素の模様方向を特定する模様方向特定処理と、
前記模様方向特定処理による前記模様方向の特定結果を基に所定の統計処理を行うことにより、前記検出対象領域において通行痕跡が存在すると推定される痕跡領域を検出する痕跡領域検出処理と、を行う
ことを特徴とする請求項1または2に記載の移動経路計画装置。
【請求項7】
前記画像処理手段は、
方向パラメータの異なる複数の空間周波数フィルタを用いて、前記撮影画像における検出対象領域をフィルタリングすることにより、前記検出対象領域についての各方向の強度分布データを生成するフィルタリング処理と、
前記フィルタリング処理によって生成された前記強度分布データを基に前記検出対象領域における各画素の模様方向を特定する模様方向特定処理と、
前記検出対象領域を奥行き方向に沿って複数に分割して得られる副検出対象領域ごとに、前記模様方向特定処理による前記模様方向の特定結果に基づいて所定の統計処理を行うことにより、前記副検出対象領域において通行痕跡が存在すると推定される痕跡領域を検出する痕跡領域検出処理と、を行う
ことを特徴とする請求項1または2に記載の移動経路計画装置。
【請求項8】
前記画像処理手段は、前記痕跡領域検出処理における前記統計処理として、
前記検出対象領域における各画素のそれぞれを投票元画素として選択し、前記投票元画素を起点としてその模様方向に平行な直線上に存在する画素、または前記投票元画素を起点としてその模様方向に沿うように設定された投票対象領域に含まれる画素に対して投票を行う投票処理と、
前記投票処理による投票結果を基に投票回数が最も大きい画素を消失点として特定する消失点特定処理と、
前記消失点の投票元画素を有効投票画素として抽出する有効投票画素抽出処理と、
前記検出対象領域において前記消失点を頂点とする領域を複数の小領域に分割し、各小領域の内、前記有効投票画素の比率が所定の閾値を超えた小領域を前記痕跡領域として特定する痕跡領域特定処理と、を行う
ことを特徴とする請求項6記載の移動経路計画装置。
【請求項9】
前記画像処理手段は、前記痕跡領域検出処理における前記統計処理として、
前記副検出対象領域における各画素のそれぞれを投票元画素として選択し、前記投票元画素を起点としてその模様方向に平行な直線上に存在する画素、または前記投票元画素を起点としてその模様方向に沿うように設定された投票対象領域に含まれる画素に対して投票を行う投票処理と、
前記投票処理による投票結果を基に投票回数が最も大きい画素を消失点として特定する消失点特定処理と、
前記消失点の投票元画素を有効投票画素として抽出する有効投票画素抽出処理と、
前記副検出対象領域において前記消失点を頂点とする領域を複数の小領域に分割し、各小領域の内、前記有効投票画素の比率が所定の閾値を超えた小領域を前記痕跡領域として特定する痕跡領域特定処理と、を行う
ことを特徴とする請求項7記載の移動経路計画装置。
【請求項10】
請求項1〜9のいずれか一項に記載の移動経路計画装置と、
移動体の動力を発生するアクチュエータと、
前記移動経路計画装置によって計画された移動経路に沿って移動体が移動するように前記アクチュエータを制御する運動制御手段と、
を備えることを特徴とする移動体制御装置。
【請求項11】
前記移動体の位置に関する情報を取得する位置情報取得手段と、
前記移動体の姿勢に関する情報を取得する姿勢情報取得手段と、
前記位置情報取得手段及び前記姿勢情報取得手段によって取得された情報を基に前記移動体の現在位置及び姿勢を推定する位置・姿勢推定手段を備え、
前記運動制御手段は、前記位置・姿勢推定手段によって推定された移動体の現在位置及び姿勢を基に前記移動体が前記移動経路を中心とする所定の許容範囲内を移動するように前記アクチュエータを制御することを特徴とする請求項10記載の移動体制御装置。
【請求項12】
請求項10または11に記載の移動体制御装置を搭載することを特徴とする移動体。
【図1】
【図2】
【図4】
【図5】
【図7】
【図8】
【図13】
【図16】
【図17】
【図18】
【図3】
【図6】
【図9】
【図10】
【図11】
【図12】
【図14】
【図15】
【図19】
【図2】
【図4】
【図5】
【図7】
【図8】
【図13】
【図16】
【図17】
【図18】
【図3】
【図6】
【図9】
【図10】
【図11】
【図12】
【図14】
【図15】
【図19】
【公開番号】特開2010−225126(P2010−225126A)
【公開日】平成22年10月7日(2010.10.7)
【国際特許分類】
【出願番号】特願2009−74796(P2009−74796)
【出願日】平成21年3月25日(2009.3.25)
【出願人】(000000099)株式会社IHI (5,014)
【Fターム(参考)】
【公開日】平成22年10月7日(2010.10.7)
【国際特許分類】
【出願日】平成21年3月25日(2009.3.25)
【出願人】(000000099)株式会社IHI (5,014)
【Fターム(参考)】
[ Back to top ]