JP2010225126A - 移動経路計画装置、移動体制御装置及び移動体 - Google Patents

Abstract

【課題】移動体の経路計画効率の向上を図ることが可能な移動経路計画装置を提供する。
【解決手段】移動体の進行方向を撮影する撮影手段と、前記撮影手段から得られる撮影画像を基に所定の画像処理を行うことにより、前記撮影画像に存在する通行痕跡を検出する画像処理手段と、前記画像処理手段によって検出された通行痕跡を基に前記移動体の移動経路を計画する経路計画手段とを備える。
【選択図】図１

Description

本発明は、移動経路計画装置、移動体制御装置及び移動体に関する。

従来では、不整地における移動体（例えば車両）の制御手法として、以下の手法が採用されていた。まず、ステレオ画像計測やレーザレンジファインダ（ＬＲＦ）等を用いて周囲の３次元形状を計測し、その計測結果に基づいて、大きな突起物等を障害物として検出する。そして、障害物から接触を避けられるだけの十分な距離を確保しつつ、且つ、経路上の起伏やカーブ等の減速要因・動揺など、運行を不安定にする要因が最小となるように車両の経路や速度等を計画し、その計画に従って車両の加減速、ステアリング角を求め、各アクチュエータによって車両のステアリング及び車輪の駆動を行う。

なお、従来の移動体制御手法として、下記特許文献１には、周囲の３次元形状を計測して行動可能範囲（環境地図）を判断し、その行動可能範囲内で経路計画を行う手法が開示されている。また、下記特許文献２には、地形センサによるセンシングで経路探索地図を作成し、複数の経路候補の中から最も平坦な経路を走行上の最適経路として選択する手法が開示されている。また、下記特許文献３には、障害物の検出から各アクチュエータの制御に至るまでの自律走行制御システムの具体的な構成例が開示されている。さらに、下記特許文献４には、将来見通せる距離が短い場合の対処の例として、逐次再計画する手法が開示されている。

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

例えば、上述したステレオ画像計測やＬＲＦ等を用いて、図１９（ａ）や（ｂ）に示すような不整地の３次元形状を計測し、その計測結果に基づいて車両の経路計画を行う場合を想定する。この場合、図１９（ａ）や（ｂ）のような画像を撮影した場所において、周囲の３次元形状が正確に得られる近傍領域（ステレオ画像計測で十分な精度が出る範囲、またはＬＲＦで計測している範囲で画像中の真ん中より下に相当する領域）は広い範囲で平坦であり、どのような経路もとりうるため、目標地点に向けて直行するような経路（図中の実線の矢印）を計画しがちである。

ところが、そのように計画した経路に沿って車両を走行させ、ある一定以上の遠方まで進んだ地点で障害物が存在して行き止まりになり、急に前方が通行不能であることが判明した場合、障害物を回避して新たに経路を計画し直す必要が生じる。このように、従来の経路計画手法では、障害物を回避可能な理想的な経路（図中の破線の矢印）と、計画された経路とが必ずしも一致せず、経路計画効率の低下を招くという問題があった。

本発明は、上述した事情に鑑みてなされたものであり、経路計画効率の向上を図ることが可能な移動経路計画装置、移動体制御装置及び移動体を提供することを目的とする。

上記課題を解決するために、本発明では、移動経路計画装置に係る第１の解決手段として、移動体の進行方向を撮影する撮影手段と、前記撮影手段から得られる撮影画像を基に所定の画像処理を行うことにより、前記撮影画像に存在する通行痕跡を検出する画像処理手段と、前記画像処理手段によって検出された通行痕跡を基に前記移動体の移動経路を計画する経路計画手段とを備えることを特徴とする。

また、移動経路計画装置に係る第２の解決手段として、上記第１の解決手段において、移動体の進行方向の３次元形状を計測する３次元計測手段と、前記３次元計測手段から得られる３次元形状の計測結果を基に地図を作成する地図作成手段と、をさらに備え、前記経路計画手段は、前記地図作成手段によって作成された地図及び前記画像処理手段によって検出された通行痕跡を基に前記移動体の移動経路を計画することを特徴とする。

また、移動経路計画装置に係る第３の解決手段として、上記第１または第２の解決手段において、前記画像処理手段は、幅を有する帯状の模様を前記通行痕跡として検出することを特徴とする。

また、移動経路計画装置に係る第４の解決手段として、上記第１または第２の解決手段において、前記画像処理手段は、幅を有し、途中で断続的になっている帯状の模様が複数、３次元空間上で平行になっている部分を前記通行痕跡として検出することを特徴とする。

また、移動経路計画装置に係る第５の解決手段として、上記第１または第２の解決手段において、前記画像処理手段は、幅を有し、途中で断続的になっている帯状の部分を前記撮影画像中から見つけ出し、最も多数の帯状部分が共有する消失点に向かっている帯状部分を前記通行痕跡として絞り込むことを特徴とする。

また、移動経路計画装置に係る第６の解決手段として、上記第１または第２の解決手段において、前記画像処理手段は、方向パラメータの異なる複数の空間周波数フィルタを用いて、前記撮影画像における検出対象領域をフィルタリングすることにより、前記検出対象領域についての各方向の強度分布データを生成するフィルタリング処理と、前記フィルタリング処理によって生成された前記強度分布データを基に前記検出対象領域における各画素の模様方向を特定する模様方向特定処理と、前記模様方向特定処理による前記模様方向の特定結果を基に所定の統計処理を行うことにより、前記検出対象領域において通行痕跡が存在すると推定される痕跡領域を検出する痕跡領域検出処理とを行うことを特徴とする。

また、移動経路計画装置に係る第７の解決手段として、上記第１または第２の解決手段において、前記画像処理手段は、方向パラメータの異なる複数の空間周波数フィルタを用いて、前記撮影画像における検出対象領域をフィルタリングすることにより、前記検出対象領域についての各方向の強度分布データを生成するフィルタリング処理と、前記フィルタリング処理によって生成された前記強度分布データを基に前記検出対象領域における各画素の模様方向を特定する模様方向特定処理と、前記検出対象領域を奥行き方向に沿って複数に分割して得られる副検出対象領域ごとに、前記模様方向特定処理による前記模様方向の特定結果に基づいて所定の統計処理を行うことにより、前記副検出対象領域において通行痕跡が存在すると推定される痕跡領域を検出する痕跡領域検出処理とを行うことを特徴とする。

また、移動経路計画装置に係る第８の解決手段として、上記第６の解決手段において、前記画像処理手段は、前記痕跡領域検出処理における前記統計処理として、前記検出対象領域における各画素のそれぞれを投票元画素として選択し、前記投票元画素を起点としてその模様方向に平行な直線上に存在する画素、または前記投票元画素を起点としてその模様方向に沿うように設定された投票対象領域に含まれる画素に対して投票を行う投票処理と、前記投票処理による投票結果を基に投票回数が最も大きい画素を消失点として特定する消失点特定処理と、前記消失点の投票元画素を有効投票画素として抽出する有効投票画素抽出処理と、前記検出対象領域において前記消失点を頂点とする領域を複数の小領域に分割し、各小領域の内、前記有効投票画素の比率が所定の閾値を超えた小領域を前記痕跡領域として特定する痕跡領域特定処理とを行うことを特徴とする。

また、移動経路計画装置に係る第９の解決手段として、上記第７の解決手段において、前記画像処理手段は、前記痕跡領域検出処理における前記統計処理として、前記副検出対象領域における各画素のそれぞれを投票元画素として選択し、前記投票元画素を起点としてその模様方向に平行な直線上に存在する画素、または前記投票元画素を起点としてその模様方向に沿うように設定された投票対象領域に含まれる画素に対して投票を行う投票処理と、前記投票処理による投票結果を基に投票回数が最も大きい画素を消失点として特定する消失点特定処理と、前記消失点の投票元画素を有効投票画素として抽出する有効投票画素抽出処理と、前記副検出対象領域において前記消失点を頂点とする領域を複数の小領域に分割し、各小領域の内、前記有効投票画素の比率が所定の閾値を超えた小領域を前記痕跡領域として特定する痕跡領域特定処理とを行うことを特徴とする。
は、２次元ガボールフィルタであることを特徴とする。

また、移動経路計画装置に係る第１０の解決手段として、上記第９の解決手段において、前記画像処理手段は、前記投票処理では、現在着目している副検出対象領域において、以前着目していた副検出対象領域について実施された前記痕跡領域特定処理によって特定された痕跡領域の終端側に、投票の重み付けを大きくした投票重み付け領域を設定することを特徴とする。

また、移動経路計画装置に係る第１１の解決手段として、上記第１０の解決手段において、前記画像処理手段は、現在着目している副検出対象領域について前記痕跡領域特定処理が終了した後に、以前着目していた副検出対象領域において、前記現在着目している副検出対象領域について実施された前記痕跡領域特定処理によって特定された痕跡領域の始端側に、投票の重み付けを大きくした投票重み付け領域を設定し、当該以前着目していた副検出対象領域について前記投票処理、消失点特定処理、有効投票画素抽出処理及び痕跡領域特定処理を再実施することで、当該以前着目していた副検出対象領域についての痕跡領域特定結果を修正することを特徴とする。

また、移動経路計画装置に係る第１２の解決手段として、上記第１０または第１１の解決手段において、前記画像処理手段は、前記奥行き方向に沿った位置に応じて前記投票重み付け領域の大きさ、及び／または前記投票の重み付け量を変化させることを特徴とする。

一方、本発明では、移動体制御装置に係る第１の解決手段として、上記の第１〜第１２の解決手段のいずれかを有する移動経路計画装置と、移動体の動力を発生するアクチュエータと、前記移動経路計画装置によって計画された移動経路に沿って移動体が移動するように前記アクチュエータを制御する運動制御手段とを備えることを特徴とする。

また、移動体制御装置に係る第２の解決手段として、上記第１の解決手段において、前記移動体の位置に関する情報を取得する位置情報取得手段と、前記移動体の姿勢に関する情報を取得する姿勢情報取得手段と、前記位置情報取得手段及び前記姿勢情報取得手段によって取得された情報を基に前記移動体の現在位置及び姿勢を推定する位置・姿勢推定手段を備え、前記運動制御手段は、前記位置・姿勢推定手段によって推定された移動体の現在位置及び姿勢を基に前記移動体が前記移動経路を中心とする所定の許容範囲内を移動するように前記アクチュエータを制御することを特徴とする。
さらに、本発明では、移動体に係る解決手段として、上記の第１または第２の解決手段を有する移動体制御装置を搭載することを特徴とする。

本発明によれば、不整地のように全体的に平坦・なだらかな地形であっても、そこある道のような通行痕跡（轍、ふみ跡など）を検出することが可能である。そして、不整地における通行痕跡は、実際に以前から何度か車両が通行していた結果生じるものであり、その延長線上は継続して通行可能なエリアになっている可能性が極めて高い。このような通行痕跡を使って計測エリア外の通行可能性が高い方向を予測することで、計測エリア外も含めて効率的な移動経路を計画することが可能となる。

本実施形態に係る移動体制御装置を搭載した車両Ｃの構成概略図である。 本実施形態に係る移動体制御装置の画像処理装置２のブロック構成図である。 本実施形態における入力画像及びその入力画像データの一例である。 本実施形態に係る画像処理装置２における信号処理装置２ｃの動作フローチャートである。 本実施形態に係る信号処理装置２ｃのフィルタリング処理に関する説明図である。 本実施形態に係る信号処理装置２ｃの模様方向特定処理に関する説明図である。 本実施形態に係る信号処理装置２ｃの痕跡領域検出処理における投票処理に関する第１説明図である。 本実施形態に係る信号処理装置２ｃの痕跡領域検出処理における投票処理に関する第２説明図である。 本実施形態に係る信号処理装置２ｃの痕跡領域検出処理における投票処理に関する第３説明図である。 本実施形態に係る信号処理装置２ｃの痕跡領域検出処理における消失点特定処理に関する説明図である。 本実施形態に係る信号処理装置２ｃの痕跡領域検出処理における有効投票画素抽出処理に関する説明図である。 本実施形態に係る信号処理装置２ｃの痕跡領域検出処理における痕跡領域特定処理に関する説明図である。 本実施形態に係る信号処理装置２ｃの痕跡領域検出処理の変形例に関する第１説明図である。 本実施形態に係る信号処理装置２ｃの痕跡領域検出処理の変形例に関する第２説明図である。 本実施形態に係る信号処理装置２ｃの痕跡領域検出処理の変形例に関する第３説明図である。 本実施形態に係る地図作成装置４によって作成された地図（３次元マップ）の一例である。 本実施形態に係る経路計画装置５によって計画された移動経路の一例である。 本実施形態に係る運動制御装置９の制御によって移動する車両Ｃの移動範囲の一例である。 従来における移動経路計画手法の説明図である。

以下、図面を参照して、本発明の一実施形態について説明する。
図１は、本実施形態に係る移動体制御装置を搭載した移動体の構成概略図である。本実施形態では、移動体として移動体制御装置の制御によって自律走行する車両Ｃを例示して説明する。なお、図１（ａ）は車両Ｃの側面図であり、図１（ｂ）は車両Ｃの上面図である。また、以下では、図１に示すように、車両Ｃの進行方向をＸ軸、Ｘ軸と直交する車両Ｃの幅方向をＹ軸、ＸＹ平面（水平面）に垂直な方向（車両Ｃの高さ方向）をＺ軸とするＸＹＺ直交座標系を設定して説明する。

この図１に示すように、車両Ｃに搭載された移動体制御装置は、撮影装置１ａ、１ｂ、画像処理装置２、３次元計測装置３、地図作成装置４、経路計画装置５、ＧＰＳ受信機６、ジャイロ７、位置・姿勢推定装置８、運動制御装置９及びアクチュエータ１０から構成されている。なお、上記の撮影装置１ａ、１ｂ、画像処理装置２、３次元計測装置３、地図作成装置４及び経路計画装置５は、本発明における移動経路計画装置を構成するものである。

撮影装置１ａ、１ｂ（撮影手段）は、例えば赤外線カメラであり、車両Ｃの上部左右に対となるように設置され、車両Ｃの進行方向を撮影して撮影画像を示す信号（画像信号）を画像処理装置２に出力する。図１に示すように、これら撮影装置１ａ、１ｂによって垂直方向（Ｚ軸方向）の視野４０°、水平方向（Ｘ軸方向）の視野６０°をカバーすることが望ましい。また、上空の情報は不要であるため、車体が振動しても水平線が撮影画像の上端より飛び出ない程度に下向きに撮影装置１ａ、１ｂを設置することが望ましい（例えばＹ軸に対して下向きに１０°傾けて設置し、水平線より下３０°、上１０°が視野に収まるように撮影を行う）。

なお、図１では図示していないが、必要に応じて赤外線の投光器を車両Ｃの上部に設置しても良い。また、本実施形態では２つの撮影装置１ａ、１ｂを用いた場合を例示しているが、撮影装置の設置数はこれに限定されず、１つの撮影装置によって十分な撮影領域を確保可能な場合は１つだけ設置すれば良く、逆に１つでは十分な撮影領域を確保できない場合には、必要に応じて複数の撮影装置を設置しても良い。また、撮影装置１ａ、１ｂとして赤外線カメラ以外のカメラを用いても良い。

画像処理装置２（画像処理手段）は、例えば画像処理専用のＰＣ（Personal Computer）であり、撮影装置１ａ、１ｂから入力される画像信号（つまり撮影画像）を基に所定の画像処理を行うことにより、撮影画像に存在する通行痕跡（例えば轍など）を検出し、その通行痕跡検出結果を経路計画装置５に出力する。以下、図２を参照して、この画像処理装置２の内部構成について詳細に説明する。

図２は、画像処理装置２のブロック構成図である。図２に示すように、画像処理装置２は、Ａ／Ｄコンバータ２ａ、記憶装置２ｂ、信号処理装置２ｃ及びＲＡＭ(Random Access Memory)２ｄから構成されている。

Ａ／Ｄコンバータ２ａは、撮影装置１ａ、１ｂから入力される画像信号をデジタルデータに変換し、入力画像（撮影画像）を示す入力画像データとして信号処理装置２ｃに出力する。例えば、撮影装置１ａ、１ｂの解像度をＶＧＡ（横６４０×縦４８０画素）と想定すると、上記の入力画像データは、入力画像を構成する横６４０×縦４８０個の各画素の輝度を示す輝度データの集合となる。なお、撮影装置１ａ、１ｂからデジタルデータ化された画像信号が出力される場合には、Ａ／Ｄコンバータ２ａを省略しても良い。

記憶装置２ｂは、例えばＨＤＤ(Hard Disk Drive)であり、画像処理用プログラムＰＧと、方向パラメータの異なる複数（ｎ個）の２次元ガボールフィルタ（以下、ガボールフィルタと略す）ＧＦ１〜ＧＦｎとを予め記憶しており、信号処理装置２ｃからの読み出し要求に応じて上記画像処理用プログラムＰＧやガボールフィルタＧＦ１〜ＧＦｎを信号処理装置２ｃに出力する。

ここで、画像処理用プログラムＰＧとは、入力画像データを基に入力画像に存在する通行痕跡を検出するための画像処理（フィルタリング処理、模様方向特定処理、強エッジ領域抽出処理、痕跡領域検出処理）を信号処理装置２ｃに実行させるためのプログラムである。

また、ガボールフィルタとは、２次元ガウス関数と２次元平面上を一方向に伝播する正弦波関数とを乗じて得られる２次元ガボール関数を利用した空間周波数フィルタであり、２次元ウェーブレットフィルタの一形態である。横方向をＸ軸、縦方向をＹ軸とする２次元座標系を想定すると、２次元ガボール関数ＧＢ（ｘ、ｙ）は下記（１）式で表される。なお、下記（１）式において、ｕ_０は波の角周波数、σはガウス関数の標準偏差（ガウス窓の幅）を示すパラメータである。また、２次元ガボール関数ＧＢ（ｘ、ｙ）は、波の方向を示すパラメータ（方向パラメータ）として角周波数ｕ_０の偏角φを有している。

上記のような２次元ガボール関数ＧＢ（ｘ、ｙ）を利用したガボールフィルタを用いて、入力画像データをフィルタリングすることにより、入力画像の周波数特性やテクスチャ（模様）方向特性を抽出することができる。具体的には、入力画像において、方向パラメータ（偏角φ）と平行なテクスチャ方向を有すると共に、周波数パラメータ（角周波数ｕ_０）に近い周波数特性を有する領域が感度良く抽出される。

本実施形態では、上述したように、方向パラメータ（角周波数ｕ_０の偏角φ）の異なるｎ個のガボールフィルタＧＦ１〜ＧＦｎを予め用意して記憶装置２ｂに記憶しておく。ここで、偏角φが等角度間隔で異なるようにガボールフィルタＧＦ１〜ＧＦｎを用意する。例えば、ｎ＝７２とすると、偏角φはπ／７２（rad）＝２．５°ずつ異なることになる。つまり、ガボールフィルタＧＦ１の方向パラメータφ１を０（rad）と設定し、ガボールフィルタＧＦ２の方向パラメータφ２をπ／７２（rad）＝２．５°と設定し、ガボールフィルタＧＦ３の方向パラメータφ３を２π／７２（rad）＝５°と設定し、以下同様に、ガボールフィルタＧＦｎの方向パラメータφｎを７１π／７２（rad）＝１７７．５°と設定する。
なお、ｎの値は入力画像の解像度や信号処理装置２ｃの処理能力に応じて適宜変更すれば良いが、例えばデジタル処理を考慮して８の倍数である３２〜９６の範囲で設定することが望ましい。

一方、各ガボールフィルタＧＦ１〜ＧＦｎの周波数パラメータ（角周波数ｕ_０）と減衰パラメータ（標準偏差σ）は、各フィルタ共通の所定値に設定する。ここで、周波数パラメータは、検出対象の通行痕跡の幅に応じて設定する。例えば、波の波長（周期）が視野上で１．５度〜４度となるように角周波数ｕ_０を計算して設定する。通路の撮影エリアが奥行き方向に１０ｍから２０ｍの領域である場合、角周波数ｕ_０が３度の視野に相当するように設定すると、撮影対象上での波の山から谷間での距離は２５ｃｍ〜５０ｃｍとなり、車輪よりやや大きいサイズとなり、複数回車輪が通過した結果にできる車輪跡に近いサイズとなる。つまり、このように周波数パラメータを設定することにより、車輪跡のような轍が通路として感度良く抽出されやすくなる。また、減衰パラメータであるσは１〜０．５波長程度で設定すれば良い。

信号処理装置２ｃは、例えばＣＰＵ（Central Processing Unit）であり、記憶装置２ｂから読み出した画像処理用プログラムＰＧに従って、入力画像データを基に所定の画像処理（フィルタリング処理、模様方向特定処理、強エッジ領域抽出処理、痕跡領域検出処理）を行うことにより、入力画像に存在する轍などの通行痕跡を検出し、その通行痕跡検出結果を経路計画装置５に出力するものである。

ここで、フィルタリング処理とは、記憶装置２ｂからガボールフィルタＧＦ１〜ＧＦｎを読み出し、各ガボールフィルタＧＦ１〜ＧＦｎを用いて入力画像における検出対象領域をフィルタリングすることにより、検出対象領域についての各方向毎（φ１〜φｎ）の強度分布データを生成する処理を指す。また、模様方向特定処理とは、フィルタリング処理によって生成された強度分布データを基に検出対象領域における各画素のテクスチャ（模様）方向を特定する処理を指す。

また、強エッジ領域抽出処理とは、検出対象領域に存在する強エッジ領域を抽出する処理を指す。また、痕跡領域検出処理とは、模様方向特定処理による各画素のテクスチャ方向の特定結果と、強エッジ領域抽出処理による強エッジ領域抽出結果とを基に所定の統計処理を行うことにより、検出対象領域において通行痕跡が存在すると推定される痕跡領域を検出する処理を指す。なお、これらの各種画像処理の詳細については後述する。

ＲＡＭ２ｄは、信号処理装置２ｃが各種画像処理を実行する上で必要なデータ（強度分布データやテクスチャ方向の特定結果、強エッジ領域抽出結果、統計データ等）や、上記の入力画像データを一時的に記憶するために使用される書換え可能な揮発性メモリである。

以上が画像処理装置２の説明であり、以下では図１に戻って説明を続ける。
３次元計測装置３（３次元計測手段）は、車両Ｃの進行方向の３次元形状を計測するレーザ・レンジ・ファインダ（ＬＲＦ）であり、車両Ｃの上部中央において下向きに所定角度傾けて設置されている。このようなＬＦＲは、Ｘ軸方向の首振り（スイング）機構を有しており、図１（ｂ）に示す走査平面ＳＰ上を走査するようにレーザ光Ｌを照射し、そのレーザ光Ｌの照射点Ｐ１、Ｐ２、・・・、Ｐｎまでの距離データｄ１、ｄ２・・・、ｄｎと、そのスイング角度データα１、α２、・・・、αｎとを、３次元形状計測結果として地図作成装置４に出力する。

なお、市販されているＬＲＦの計測可能範囲は、悪条件環境下において３０ｍ程度であるため、３次元計測装置３として使用するＬＦＲも３０ｍ先の３次元形状を計測できるように設置角度や設置高さ等を設定することが望ましい。また、ＬＦＲをＺ軸方向にスイングさせながらレーザ光Ｌを走査することで３次元形状を計測するようにしても良い。また、３次元計測装置３としてはＬＲＦ以外の装置、例えばステレオカメラ等を用いても良い。

地図作成装置４（地図作成手段）は、例えば３次元マップ生成装置（３次元マップ作成専用のＰＣ（Personal Computer））であり、３次元計測装置３から得られる３次元形状計測結果（距離データｄ１、ｄ２・・・、ｄｎ、スイング角度データα１、α２、・・・、αｎ）を基に、車両Ｃの進行方向の地図（３次元マップ）を作成し、その３次元マップデータを経路計画装置５に出力する。なお、ＬＦＲから得られる距離データ及びスイング角度データから３次元マップを作成する手法は既に公知であるため、本実施形態では詳細な説明を省略する。
また、地図作成装置４としては、３次元マップ生成装置以外にも、荒さ・起伏マップ作成装置を利用することもでき、この場合には起伏の量を計測した地面の高さを微分した値を、上空から見た鳥瞰図的な座標にプロットして地図を作成する。

経路計画装置５（経路計画手段）は、例えば経路計画専用のＰＣ（Personal Computer）であり、画像処理装置２から得られる通行痕跡検出結果と、地図作成装置４から得られる３次元マップデータとを基に、車両Ｃの移動経路を計画し、その経路計画データを運動制御装置９に出力する。なお、この移動経路の計画手法の詳細については後述する。

ＧＰＳ受信機６（位置情報取得手段）は、車両Ｃの上部に設置されており、ＧＰＳ（Global Positioning System:全地球測位システム）を利用して車両Ｃの位置に関する情報として経度、緯度、高度を取得し、その取得結果を車両位置データとして位置・姿勢推定装置８に出力する。ジャイロ７（姿勢情報取得手段）は、車両Ｃの所定箇所に設置された３軸ジャイロセンサであり、車両ＣのＸ軸回り（ピッチ）の回転角度及び角速度と、Ｙ軸回り（ロール）の回転角度及び角速度と、Ｚ軸回り（ヨー）の回転角度及び角速度とを、車両Ｃの姿勢情報として取得し、その取得結果を車両姿勢データとして位置・姿勢推定装置８に出力する。

位置・姿勢推定装置８（位置・姿勢推定手段）は、例えば位置・姿勢推定専用のＰＣ（Personal Computer）であり、ＧＰＳ受信機６から得られる車両位置データと、ジャイロ７から得られる車両姿勢データと、後述するアクチュエータ１０から得られる車両Ｃの移動量に関するフィードバックデータとを基に、車両Ｃの現在位置及び姿勢を推定し、その推定結果を現在位置・姿勢データとして運動制御装置９に出力する。

運動制御装置９（運動制御手段）は、例えば車両Ｃの運動制御専用のＰＣ（Personal Computer）であり、経路計画装置５から得られる経路計画データと、位置・姿勢推定装置８から得られる現在位置・姿勢データとを基に、車両Ｃが計画された移動経路に沿って移動するように、且つ移動経路を中心とする所定の許容範囲内を移動するようにアクチュエータ１０を制御する。

アクチュエータ１０は、車両Ｃの車輪を駆動するためのモータ、ステアリングを駆動するためのモータ、ブレーキを駆動するためのモータや、これらモータの駆動状態を計測するセンサ（例えばステアリングの回転量を計測するポテンショメータやロータリエンコーダ、車輪の回転量を計測するロータリエンコーダ等）、及びそれらとフィードバックループを構成しつつ、運動制御装置９の制御によって各種モータに駆動電圧または電流を供給するモータアンプなどから構成されている。また、各種センサによって計測されたデータは、車両Ｃの移動量に関するフィードバックデータとして位置・姿勢推定装置８にフィードバックされる。

なお、本実施形態では、画像処理装置２、地図作成装置４、経路計画装置５、位置・姿勢推定装置８及び運動制御装置９をそれぞれ独立したＰＣによって構成した場合を例示したが、各装置の機能を１つのＰＣに集約するような構成としても良い。

次に、上記のように構成された車両Ｃ（特に移動体制御装置）の動作について詳細に説明する。

（通行痕跡検出動作）
まず、画像処理装置２による通行痕跡検出動作について説明する。移動体制御装置の動作中において、撮影装置１ａ、１ｂは、所定のカメラレートで車両Ｃの進行方向を撮影し、撮影画像を示す信号（画像信号）を画像処理装置２に出力する。画像処理装置２に入力された画像信号は、Ａ／Ｄコンバータ２ａによって入力画像データに変換された後、信号処理装置２ｃに転送される。

ここで、車両Ｃは川原を走行しており、撮影装置１ａ、１ｂから図３（ａ）に示すような轍が通行痕跡として写り込んだ撮影画像が得られたものと想定する。また、以下では、図３（ｂ）に示すように、撮影画像の水平方向をＸ軸方向、垂直方向をＹ軸方向とし（このＸＹ軸は図１に示すＸＹＺ直交座標系とは無関係である）、撮影画像の座標（Ｘ、Ｙ）＝（０、０）〜（６３９、４７９）の各画素に対応する輝度データをＢ（０、０）〜Ｂ（６３９、４７９）と表記する。つまり、入力画像データは、これら輝度データＢ（０、０）〜Ｂ（６３９、４７９）の集合である。このような入力画像データは、信号処理装置２ｃによってＲＡＭ２ｄに一旦記憶される。

図４は、信号処理装置２ｃの動作フローチャートである。この図４に示すように、まず、信号処理装置２ｃは、ＲＡＭ２ｃから入力画像（撮影画像）における検出対象領域に相当する入力画像データを読み出すと共に、記憶装置２ｂからガボールフィルタＧＦ１〜ＧＦｎを読み出し、各ガボールフィルタＧＦ１〜ＧＦｎを用いて入力画像データをフィルタリングすることにより、各方向の強度分布データを生成する（ステップＳ１：フィルタリング処理）。

本実施形態では、入力画像の全領域が上記の検出対象領域に設定されている場合を例示して説明する。つまり、ＲＡＭ２ｄから入力画像の全領域に相当する入力画像データが読み出される。なお、入力画像から水平線を抽出可能である場合には、その水平線より下の領域（つまり地面の領域）を検出対象領域として設定しても良い。このように、確実に通行痕跡が存在すると推定される領域のみを検出対象領域とすることにより、以下で説明する各種画像処理の処理負荷を軽減でき、処理時間を短縮することができる。

以下、図５を参照して上記ステップＳ１について具体的に説明する。図５（ａ）に示すように、信号処理装置２ｃは、ガボールフィルタＧＦ１〜ＧＦｎを用いたフィルタリング処理として、入力画像において、ある画素を中心画素とする畳み込み領域に含まれる各画素の輝度データと、ガボールフィルタ、つまり２次元ガボール関数ＧＢ（ｘ、ｙ）との畳み込み演算を行い、その演算結果をガボールフィルタの方向パラメータφに対応する中心画素の強度として取得する。

つまり、信号処理装置２ｃは、まず、ある画素を中心画素とする畳み込み領域に含まれる各画素の輝度データとガボールフィルタＧＦ１との畳み込み演算を行い、その演算結果をガボールフィルタＧＦ１の方向パラメータφ１に対応する中心画素の強度として取得する。ここで、各ガボールフィルタＧＦ１〜ＧＦｎを、実数部のフィルタと虚数部のフィルタとに分けて作成しておくことにより、実数部のフィルタを用いて得られた強度と、虚数部のフィルタを用いて得られた強度とをそれぞれ２乗して加算し、その加算値の平方根を求めることで最終的な強度を取得することができる。

続いて、信号処理装置２ｃは、ガボールフィルタＧＦ２に切り替えて同様に畳み込み演算を行い、その演算結果をガボールフィルタＧＦ２の方向パラメータφ２に対応する中心画素の強度として取得する。以下、同様に、信号処理装置２ｃは、ガボールフィルタを順次切り替えながら畳み込み演算を行うことにより、その演算結果を各ガボールフィルタＧＦ１〜ＧＦｎの方向パラメータφ１〜φｎに対応する中心画素の強度として取得する。

信号処理装置２ｃは、上記のようなフィルタリング処理を、入力画像の座標（０、０）〜（６３９、４７９）の各画素のそれぞれについて行うことにより、図５（ｂ）に示すような各方向の強度分布データを生成してＲＡＭ２ｄに記憶する。なお、図５（ｂ）では、座標（０、０）〜（６３９、４７９）の各画素のそれぞれを中心画素として得られた強度データをＩ（０、０）〜Ｉ（６３９、４７９）として表記している。

なお、図５（ａ）からわかるように、入力画像の端部に近い画素を中心画素とする場合、畳み込み領域が入力画像からはみ出てしまうため、この場合には予め設定した輝度データ（例えば白データ等）を入力画像からはみ出ている畳み込み領域の輝度データとして使用すれば良い。また、畳み込み領域の大きさは、検出対象の通行痕跡の大きさに応じて適宜変更すれば良いが、例えば幅２５ｃｍ〜５０ｃｍの車輪跡に相当する轍を検出対象とする場合には、その幅に相当する画素数（３２画素）以上の大きさ、例えば５０×５０画素の大きさとすることが望ましい。

次に、上記のようなステップＳ１が終了すると、信号処理装置２ｃは、ＲＡＭ２ｄから各方向の強度分布データを読み出し、それら強度分布データを基に入力画像における各画素のテクスチャ（模様）方向を特定する（ステップＳ２：模様方向特定処理）。具体的には、例えば座標（０、０）の画素に着目した場合、各方向の強度分布データの中から強度データＩ（０、０）が最も大きいものを探索し、その最大の強度データＩ（０、０）を含む強度分布データに対応する方向を、座標（０、０）の画素のテクスチャ方向として特定する。例えば、方向φ３に対応する強度分布データに最大の強度データＩ（０、０）が含まれている場合、方向φ３が座標（０、０）の画素のテクスチャ方向となる。

信号処理装置２ｃは、上記のようなテクスチャ方向の特定処理を、座標（０、０）〜（６３９、４７９）の各画素について行うことにより、各画素のテクスチャ方向を特定し、その特定結果をＲＡＭ２ｄに記憶する。図６（ａ）は各画素のテクスチャ方向の特定結果であり、図６（ｂ）は入力画像とテクスチャ方向の特定結果とを重ねて表示したものである。なお、この図６では、説明の便宜上、テクスチャ方向の特定結果を、テクスチャ方向を指し示すベクトル線で表しているが、実際にはテクスチャ方向の特定結果は、その方向を表す値（例えばテクスチャ方向がφ１であればφ１の値）によって構成されている。

不整地上に存在する轍などの通行痕跡は、車両が複数回その場所を走行することにより、地面や草むらが踏み起こされたり、踏み固められたりした結果残るものであるため、図３に示すように、その痕跡は入力画像上において不連続且つ不明瞭な帯状やスジ状の疎らな領域として観察される。そこで、上述した入力画像の波の成分を抽出する機能を有するステップＳ１及びステップＳ２を実施することより、通行痕跡を形成する模様の間隔は不定でなだらかではあるが、通行痕跡の進行方向と平行な波の成分として捕らえることができる。

次に、上記のようなステップＳ２が終了すると、信号処理装置２ｃは、入力画像データを基に所定のエッジ検出処理を行うことにより、入力画像に存在する強エッジ領域を抽出し、その抽出結果をＲＡＭ２ｄに記憶する（ステップＳ３：強エッジ領域検出処理）。ここで、エッジ検出処理としては公知の技術を採用することができる。例えば、Ｘ軸方向のソベルフィルタとＹ軸方向のソベルフィルタを適用して、それらの強度の２乗和が一定の閾値を超えた画素を探索し、その閾値を超えた画素から一定距離以内の領域を強エッジ領域として抽出する。

次に、上記のようなステップＳ３が終了すると、信号処理装置２ｃは、模様方向特定処理による各画素のテクスチャ方向の特定結果と、強エッジ領域抽出処理による強エッジ領域抽出結果とを基に所定の統計処理を行うことにより、入力画像において通行痕跡が存在すると推定される痕跡領域を検出する（ステップＳ４：痕跡領域検出処理）。具体的には、信号処理装置２ｃは、ステップＳ４における統計処理として、投票処理（ステップＳ４ａ）、消失点特定処理（ステップＳ４ｂ）、有効投票画素抽出処理（ステップＳ４ｃ）及び痕跡領域特定処理（ステップＳ４ｄ）を行う。以下、それぞれの処理について詳細に説明する。

＜投票処理＞
まず、信号処理装置２ｃは、ステップＳ４ａの投票処理として、入力画像における各画素のそれぞれを投票元画素として順次選択し、当該投票元画素を起点としそのテクスチャ方向に平行な直線上に存在する画素に対して投票を行う。具体的には、信号処理装置２ｃは、まず、図７に示すように、ＲＡＭ２ｄのメモリ空間上に、投票回数を集計するための投票集計領域を入力画像と同サイズ（つまり、６４０×４８０画素数分の投票回数を集計可能なサイズ）で確保する。

そして、信号処理装置２ｃは、図８（ａ）に示すように、ある画素を投票元画素として選択した場合、模様方向特定処理による各画素のテクスチャ方向の特定結果から投票元画素のテクスチャ方向を把握し、投票元画素を起点としそのテクスチャ方向に平行な直線上に存在する画素（投票対象画素）に対して投票を行い、その投票結果を図８（ｂ）に示すようにＲＡＭ２ｄ上の投票集計領域に反映する。つまり、図８（ｂ）からわかるように、投票集計領域において投票対象画素に対応する記憶領域に投票値「１」が加算される。

信号処理装置２ｃは、上記のような投票処理を各画素について行い、その投票結果を順次ＲＡＭ２ｄ上の投票集計領域に反映する。ここで、信号処理装置２ｃは、上記のような投票処理を行う際、強エッジ領域抽出処理によって抽出された強エッジ領域の周辺（たとえば２画素以内）に含まれる画素を投票元画素から除外する。この理由については後述する。

なお、上述の投票処理では、投票元画素を起点としそのテクスチャ方向に平行な直線上に存在する投票対象画素に対して投票を行う場合を説明したが、これに限らず、投票元画素を起点としてそのテクスチャ方向に沿うように設定された投票対象領域に含まれる画素（投票対象画素）に対して投票を行うようにしても良い。この投票対象領域は、例えば図９（ａ）に示すように、テクスチャ方向に平行な帯状（長方形状）のものでも良いし、また、図９（ｂ）に示すように、円弧状あるいは三角形状のものでも良い。

ここで、図９（ａ）に示すような帯状の投票対象領域を設定する場合には、上述したテクスチャ方向に平行な直線上に存在する投票対象画素に対して投票を行う場合と同様に、投票対象領域に含まれる投票対象画素のそれぞれに対して平等に投票すれば良い。つまり、ＲＡＭ２ｄ上の投票集計領域において、各投票対象画素に対応する記憶領域に平等に投票値「１」を加算する。

一方、図９（ｂ）に示すような円弧状あるいは三角形状の投票対象領域を設定する場合には、投票元画素と投票対象画素との間の距離に応じて投票値を変更することが望ましい。具体的には、投票元画素から遠い距離に位置する投票対象画素の投票値を小さくする（例えば、投票値＝１／投票元画素からの距離）。つまり、投票元画素に近い位置の投票対象画素の投票値は「１」または「１」に近い値となるが、投票元画素から遠い位置の投票対象画素の投票値は「０．５」等の小数点以下の値となる。さらには、投票対象領域において同じ距離に位置する投票対象画素の総投票値が「１」となるように各投票対象画素の投票値を決定することが望ましい。つまり、例えば同じ距離に位置する投票対象画素が４つ存在すると仮定すると、それら４つの各投票対象画素に対する投票値をそれぞれ「０．２５」とする。

このように円弧状あるいは三角形状の投票対象領域を設定した場合に、投票元画素と投票対象画素との間の距離に応じて投票値を変更する理由は、投票元画素から遠い位置にある投票対象画素は、他の投票対象領域と重なり合う確率が高く、近い位置にある投票対象画素と比べて投票されやすい傾向にあり、後述する消失点を正確に特定することが困難となるためである。また、円弧状あるいは三角形状の投票対象領域を設定した場合、その中心角度（投票元画素を起点とする角度）は、ガボールフィルタＧＦ１〜ＧＦｎの方向パラメータφの変化分（つまりπ／７２（rad）＝２．５°）と同一にすることが望ましい。

＜消失点特定処理＞
続いて、信号処理装置２ｃは、ステップＳ４ｂの消失点特定処理として、図１０に示すように、投票処理による投票結果（つまり、ＲＡＭ４０上の投票集計領域に集計された投票回数）を基に、投票回数が最も大きい画素を入力画像における消失点として特定する。轍などの通行痕跡の波模様は車両Ｃから見ると完全に平行ではないが同じ消失点を共有していると推測される。このため、上記のような投票処理によって消失点と推測される位置（画素）に投票を行い、統計的に通行痕跡と平行している波模様が多い検出結果を活用することにより、消失点を特定することができる。

ここで、入力画像において通行痕跡以外に、例えば木や電柱、看板などのコントラストの高い物体が写り込んでいた場合、それらの模様は上記の投票回数の集計に大きな影響を及ぼすことになる。そこで、上述したように、投票処理を行う際には、強エッジ領域抽出処理によって抽出された強エッジ領域に含まれる画素を投票元画素から除外することにより、木や電柱、看板などのコントラストの高い物体の延長線上が消失点として特定されることを防止することができる。

＜有効投票画素抽出処理＞
続いて、信号処理装置２ｃは、ステップＳ４ｃの有効投票画素抽出処理として、消失点の投票元画素を有効投票画素として抽出する。つまり、消失点を向いているテクスチャ方向を有する画素が有効投票画素として抽出されることになる。図１１に、有効投票画素の抽出結果を示す。図１１において白領域が有効投票画素の集合領域である。

＜痕跡領域特定処理＞
続いて、信号処理装置２ｃは、ステップＳ４ｄの痕跡領域特定処理として、図１２に示すように、入力画像において消失点を頂点とする領域（この領域は円弧状でも良いし、三角形状でも良い）を複数の小領域に分割し、各小領域の内、有効投票画素の比率が所定の閾値を超えた小領域を通行痕跡が存在すると推定される痕跡領域として特定する。ここで、有効投票画素の比率は、小領域と有効投票画素との面積比率、つまり、小領域に含まれる有効投票画素数を小領域に含まれる全画素数で除算することで求めることができる。

入力画像において轍などの通行痕跡が写り込んでいる領域では、周囲と比べて高い比率で消失点を向いているテクスチャを含んでいると推測される。そこで、上記のような有効投票画素抽出処理及び痕跡領域特定処理を行うことにより、入力画像において通行痕跡が存在する痕跡領域と他の領域とを判別することができる。

なお、痕跡領域は轍などの通行痕跡に沿って検出されるものであるので、隣接する痕跡領域は、ある規則性を持って連続的または断続的に連なっているはずである。そこで、入力画像において、ある規則性を持った痕跡領域の集まりから孤立している痕跡領域はノイズとして捉え、そのような孤立領域は痕跡領域から除去することにより、通行痕跡の誤検出を防止することができる。

ところで、上記説明は、図３に示すように、通行痕跡が水平線に向かって一直線に延びているような入力画像が得られた場合を想定したものである。しかしながら、図１３に示すように、入力画像中の通行痕跡がカーブしているような場合には、入力画像の奥行き毎に消失点が水平方向に変化することになる。このような場合、上述したように入力画像の全領域（検出対象領域の全領域）を一度に処理すると、正確な消失点を得ることができなくなる。

そこで、このような問題に対処するために、上記のステップＳ４の処理（投票処理、消失点特定処理、有効投票画素抽出処理及び痕跡領域特定処理）を、図１３に示すように、検出対象領域（ここでは入力画像の全領域）を奥行き方向に沿って複数に分割して得られる副検出対象領域ごとに行うようにしても良い。このように、副検出対象領域毎に消失点を求め、その対応する奥行きごとに消失点に適合する有効投票画素を求めることで、それぞれの奥行きに対応した消失点と痕跡領域の切り出しを行うことができる。

ここで、２次元ウェーブレットフィルタを用いた場合、入力画像上で強い方向性を有する領域が轍などの痕跡領域として検出されるため、轍の外に生えている草や樹木、人工物の平行な特徴を誤って検出してしまう可能性がある。そこで、上記のように、奥行き方向に沿って複数に分割して得られる副検出対象領域ごとに痕跡領域の特定を行う場合、以下に説明する処理を加えることで検出精度の向上を図ることができる。

実験の結果、轍検出（痕跡領域検出）の精度は各地点までの奥行き方向の距離に依存し、手前ほど精度が高いことが判明した。また、検出済みの轍（痕跡領域）と連続した領域はやはり轍（痕跡領域）である可能性が大きいと考えられる。そこで、入力画像を手前から奥へ数段階に分割し、最も手前側の副検出対象領域に存在する轍（痕跡領域）を最初に検出して、それを基準に以遠の轍（痕跡領域）を検出することとする。

図１４に示すように、まず、奥行き方向の最も手前側の副検出対象領域（ここでは第１副検出対象領域Ｗ１とする）について、上記実施形態で説明したステップＳ４の処理（投票処理、消失点特定処理、有効投票画素抽出処理及び痕跡領域特定処理）を行うことで第１副検出対象領域Ｗ１における痕跡領域特定結果を得る。なお、図１４では、痕跡領域の特定結果を第１副検出対象領域Ｗ１の消失点に向かう連続した直線として表している。

続いて、次段の副検出対象領域（ここでは第２副検出対象領域Ｗ２とする）について上記ステップＳ４の処理を行うが、この時の投票処理では、現在着目している副検出対象領域（つまり第２副検出対象領域Ｗ２）において、図１４に示すように、以前着目していた副検出対象領域（つまり第１副検出対象領域Ｗ１）について実施された痕跡領域特定処理によって特定された痕跡領域の終端側に、投票の重み付けを大きくした投票重み付け領域Ｗａを設定する。このような投票重み付け領域Ｗａは、痕跡領域の特定結果を表す各直線上のそれぞれの終端部を中心に設定した所定サイズの矩形領域から構成されている。この矩形領域の１つ当たりのサイズは、縦：０．０８×画素高さ、横：０．０３×画素幅、程度とする。

このように投票重み付け領域Ｗａを設定した第２副検出対象領域Ｗ２について上記ステップＳ４の投票処理を行う場合、投票重み付け領域Ｗａに存在する投票対象画素に対する投票には通常の３倍程度の重み付けを行う。例えば、１段目の第１副検出対象領域Ｗ１の投票処理において投票対象画素に対する投票値を「１」とした場合、第２副検出対象領域Ｗ２の投票重み付け領域Ｗａに存在する投票対象画素に対する投票値は「３」とする。

このような第２副検出対象領域Ｗ２についてステップＳ４の投票処理、消失点特定処理、有効投票画素抽出処理及び痕跡領域特定処理を行うことにより、前段の第１副検出対象領域Ｗ１について特定された痕跡領域の終端が、次段の第２副検出対象領域Ｗ２の痕跡領域の起点となりやすくなる。言い換えれば、前段の第１副検出対象領域Ｗ１の痕跡領域と次段の第２副検出対象領域Ｗ２の痕跡領域とが連続した痕跡領域として検出されやすくなり、入力画像中に轍などと平行に生えた草や樹木、人工物等が存在する場合であっても、精度良く轍などの痕跡領域を検出することができる。

上記のように第２副検出対象領域Ｗ２について痕跡領域の特定結果を得た後は、次段（３段目）の副検出対象領域について第２副対象領域Ｗ２と同様の処理を行う。なお、３段目の副検出対象領域では、投票重み付け領域を構成する矩形領域の１つ当たりのサイズを、２段目の第２副検出対象領域Ｗ２の半分、つまり、縦：０．０４×画素高さ、横：０．０１５×画素幅、程度とする。

また、３段目の副検出対象領域について上記ステップＳ４の投票処理を行う場合、投票重み付け領域に存在する投票対象画素に対する投票には通常の５倍程度の重み付けを行う。例えば、１段目の第１副検出対象領域Ｗ１の投票処理において投票対象画素に対する投票値を「１」とした場合、３段目の副検出対象領域の投票重み付け領域に存在する投票対象画素に対する投票値は「５」とする。このように奥行き方向に沿った位置に応じて投票重み付け領域の大きさ及び投票の重み付け量を変化させることにより、より精度良く痕跡領域の連続性を確保することができる。

以降、同様な処理を奥行き方向の最も遠い副検出対象領域まで行うことにより、各段の副検出対象領域で特定された痕跡領域が連続した痕跡領域として精度良く検出され、轍などの車両の走行痕を正確に検出することが可能となる。なお、４段目以降の副検出対象領域では、投票重み付け領域を構成する矩形領域の１つ当たりのサイズ及び投票の重み付け量を３段目の副検出対象領域と同一にすることが好ましい。

図１５（ａ）は、投票重み付け領域を使用しない場合における痕跡領域の特定結果を示すものである。この図１５（ａ）に示すように、投票重み付け領域を使用しない場合、本来、痕跡領域とすべき領域とは異なる領域を痕跡領域として特定してしまっていることがわかる。また、図１５（ｂ）は、投票重み付け領域を使用した場合における痕跡領域の特定結果を示すものである。この図１５（ｂ）に示すように、投票重み付け領域を使用した場合、本来、痕跡領域とすべき領域を正しく痕跡領域として特定していることがわかる。

なお、例えば、現在着目している副検出対象領域（例えば第２副検出対象領域Ｗ２）について痕跡領域特定処理が終了した後に、以前着目していた副検出対象領域（第１副検出対象領域Ｗ１）において、第２副検出対象領域Ｗ２について実施された痕跡領域特定処理によって特定された痕跡領域の始端側に、投票の重み付けを大きくした投票重み付け領域を設定し、当該第１副検出対象領域Ｗ１についてステップＳ４の投票処理、消失点特定処理、有効投票画素抽出処理及び痕跡領域特定処理を再実施することで、当該第１副検出対象領域Ｗ１についての痕跡領域特定結果を修正することも可能である。

以上のように、画像処理装置２における信号処理装置２ｃは、上述した画像処理を実施することによって、入力画像において通行痕跡が存在すると推定される痕跡領域を検出し、その検出結果を通行痕跡検出結果として経路計画装置５に出力する。なお、上述した画像処理を、カメラレートで撮影装置１ａ、１ｂから送られる撮影画像毎に実施する必要はなく、毎秒２枚から１０枚程度の撮影画像について実施すれば良い。

（３次元マップ作成動作）
続いて、地図作成装置４による３次元マップ作成動作について説明する。移動体制御装置の動作中において、３次元計測装置３（ＬＲＦ）は、図１（ｂ）に示す走査平面ＳＰ上を走査するようにレーザ光Ｌを照射し、そのレーザ光Ｌの照射点Ｐ１、Ｐ２、・・・、Ｐｎまでの距離データｄ１、ｄ２・・・、ｄｎと、そのスイング角度データα１、α２、・・・、αｎとを、３次元形状計測結果として地図作成装置４に出力する。ここで、図１６（ａ）に示すように、車両Ｃを走行させながらレーザ光Ｌを走査することで３次元形状を計測しても良いし、ＬＦＲをＺ軸方向にスイングさせながらレーザ光Ｌを走査することで３次元形状を計測しても良い。

地図作成装置４は、３次元計測装置３から得られる３次元形状計測結果（距離データｄ１、ｄ２・・・、ｄｎ、スイング角度データα１、α２、・・・、αｎ）を基に、図１６（ｂ）に示すように、車両Ｃの進行方向の３次元マップを作成し、その３次元マップデータを経路計画装置５に出力する。なお、ＬＦＲから得られる距離データ及びスイング角度データから３次元マップを作成する手法は既に公知であるため、本実施形態では詳細な説明を省略する。

（経路計画動作）
続いて、経路計画装置５による経路計画動作について説明する。経路計画装置５は、画像処理装置２から得られる通行痕跡検出結果と、地図作成装置４から得られる３次元マップデータとを基に、轍（通行痕跡）に沿った方向、且つ轍から一定以内の距離を保つ範囲の中という条件の下、周囲の３次元形状から最適な移動経路を計画する。

より具体的には、経路計画装置５は、移動経路上の起伏、および移動経路上の各地点でのステアリング量など走行の安定性を低下させる要因をその度合いに応じたコスト値に変換し、速度・回転半径などの車体性能の制約条件を満たしつつ、そのコストが最小となるように移動経路を計画する。もしくは、移動経路上の起伏度合いとステアリングからそこで許容されている最大速度を求め、その移動経路上での最大速度に近くなるような加減速を行うことをシミュレーションし、その到達時間が最短となる移動経路を計画しても良い。

また、作成された３次元マップから周囲の３次元形状の変化を監視し、大きな変化がない場合には、近傍は再計画を行わず、遠方（たとえば計画を行う範囲の１／２以上遠方）のみを再計画するようにしても良い。これにより、近傍の軌道が再計画により細々変動したり軌道が振動したりして移動経路が不安定になることを防ぐことができる。一方、３次元マップや轍などの通行痕跡検出結果が大きく変化した場合には、全経路での再計画を行うことが望ましい。さらに、轍に沿った範囲で安定に走行できる条件での計画ができなかった場合には、モードを轍を考慮しないモード（つまり画像処理装置２から得られる通行痕跡検出結果を無視するモード）に切り替え、従来技術と同様に、３次元マップのみを用いて最適な移動経路を計画することが望ましい。
以上のような手法により、経路計画装置５は、図１７に示すような車両Ｃの移動経路を計画し、その経路計画データを運動制御装置９に出力する。

（車両Ｃの現在位置及び姿勢の推定動作）
続いて、位置・姿勢推定装置８による車両Ｃの現在位置及び姿勢の推定動作について説明する。移動体制御装置の動作中において、ＧＰＳ受信機６は、車両Ｃの位置に関する情報として経度、緯度、高度を取得し、その取得結果を車両位置データとして位置・姿勢推定装置８に出力する。また、ジャイロ７は、車両ＣのＸ軸回り（ピッチ）の回転角度及び角速度と、Ｙ軸回り（ロール）の回転角度及び角速度と、Ｚ軸回り（ヨー）の回転角度及び角速度とを、車両Ｃの姿勢情報として取得し、その取得結果を車両姿勢データとして位置・姿勢推定装置８に出力する。さらに、アクチュエータ１０の各種センサによって計測されたデータは、車両Ｃの移動量に関するフィードバックデータとして位置・姿勢推定装置８にフィードバックされる。

位置・姿勢推定装置８は、ＧＰＳ受信機６から得られる車両位置データと、ジャイロ７から得られる車両姿勢データと、アクチュエータ１０から得られる車両Ｃの移動量に関するフィードバックデータとに加えて、ＧＰＳ受信機６で受信可能な衛星数から求まる車両位置の計測誤差、及び不整地で事前に測定した車輪の滑りから求まる計測誤差に基づいて、尤もらしい車両Ｃの現在位置及び姿勢を推定し、その推定結果を現在位置・姿勢データとして運動制御装置９に出力する。なお、この推定にはカルマンフィルタやパーティクルフィルタを用いることで、より高精度に車両Ｃの現在位置及び姿勢を求めることが可能になる。

（車両Ｃの運動制御動作）
続いて、運動制御装置９による車両Ｃの運動制御動作について説明する。運動制御装置９は、経路計画装置５から得られる経路計画データと、位置・姿勢推定装置８から得られる現在位置・姿勢データとを基に、車両Ｃが計画された移動経路に沿って移動するように、且つ移動経路を中心とする所定の許容範囲内を移動するようにアクチュエータ１０を制御する。

具体的には、まず、計画された移動経路及び速度となるように、車両Ｃの現在位置及び姿勢を基に計画された移動経路に対するズレ量を加算し、所定の許容範囲内で移動経路に沿うように車両Ｃの目標旋回半径を決定する（移動経路に十分近ければ、移動経路の旋回半径を目標旋回半径に決定する）。そして、目標旋回半径の決定後、予め作成しておいたステアリング角・速度と旋回半径とを対応付けるテーブルに基づいて、現在速度及び目標旋回半径から実施すべきステアリング角を決定する。運動制御装置９は、上記のように決定したステアリング角となるようにアクチュエータ１０に対してステアリングの回転量を指示する。

一方、現在の旋回半径、及び近傍（１０ｍ程度）の移動経路の旋回半径の最も厳しい（半径の小さい）値から車両Ｃが安全に旋回するための最大速度を求め（車両Ｃの物理モデルと砂地などの摩擦係数を用いてシミュレーションを行って事前に求めておく）、最大速度と計画された速度との小さい方を車両Ｃの目標速度とする。運動制御装置９は、上記のように決定した目標速度と現在の速度とを基に、ＰＩＤ制御などを用いて制御値を決定し、アクチュエータ１０の車輪を駆動するためのモータを制御する。
なお、移動経路に対するズレ量が大きくなった場合には、経路計画装置５に対して移動経路の再計画を指示し、以前の移動経路に依存せずに、現在位置からスムーズな移動経路を計画することが望ましい。

以上説明したような運動制御装置９による運動制御動作によって、図１８に示すように、車両Ｃは計画された移動経路に沿って移動するように、且つ移動経路を中心とする所定の許容範囲内を移動するように制御されることになる。

以上のように、本実施形態によれば、不整地のように全体的に平坦・なだらかな地形であっても、そこある道のような通行痕跡（轍、ふみ跡など）を検出することが可能である。そして、不整地における通行痕跡は、実際に以前から何度か車両が通行していた結果生じるものであり、その延長線上は継続して通行可能なエリアになっている可能性が極めて高い。このような通行痕跡を使って計測エリア外の通行可能性が高い方向を予測することで、計測エリア外も含めて効率的な移動経路を計画することが可能となる。

なお、本発明は上記実施形態に限定されず、以下のような変形例が考えられる。
（１）上記実施形態では、各ガボールフィルタＧＦ１〜ＧＦｎの周波数パラメータ（角周波数ｕ_０）を固定値とする場合を例示した。これは、通路の撮影エリアが奥行き方向に１０ｍから２０ｍの領域である場合、角周波数ｕ_０が視野中の３度に相当するように設定すると、撮影対象上での波の山から谷間での距離は２５ｃｍ〜５０ｃｍとなり、車輪よりやや大きいサイズとなり、複数回車輪が通過した結果にできる車輪跡に近いサイズとなるためである。しかしながら、図３の入力画像からわかるように、轍などの通行痕跡の幅は、奥行き方向の位置によって変化する。そこで、入力画像における奥行き方向の位置に応じて、各ガボールフィルタＧＦ１〜ＧＦｎの周波数パラメータを変更することが望ましい。

具体的には、記憶装置２ｂに、入力画像における奥行き方向の位置に応じた周波数パラメータを有すると共に方向パラメータの異なるガボールフィルタを予め記憶しておき、信号処理装置２ｃに、記憶装置２ｂから検出対象領域におけるフィルタリング対象位置（つまり図５中の中心画素の位置）に対応する周波数パラメータを有するガボールフィルタを読み出してフィルタリング処理する機能を持たせれば良い。

（２）上記実施形態では、空間周波数フィルタとして２次元ガボールフィルタを用いた場合を例示したが、これに限らず、その他の２次元ウェーブレットフィルタや２次元窓付きフーリエ変換を利用したフィルタなどを空間周波数フィルタとして用いても良い。

（３）また、上記実施形態では、入力画像に存在する通行痕跡を検出する画像処理手法として、空間周波数フィルタを用いた場合を例示して説明したが、これに限定されず、通行痕跡を検出可能な画像処理手法であれば他の手法を採用しても良い。すなわち、撮影画像の中から幅を有する帯状の模様を通行痕跡として検出する手法、または、幅を有し、途中で断続的になっている帯状の模様が複数、３次元空間上で平行になっている部分を通行痕跡として検出する手法、或いは、幅を有し、途中で断続的になっている帯状の部分を撮影画像中から見つけ出し、最も多数の帯状部分が共有する消失点に向かっている帯状部分を通行痕跡として絞り込む手法であれば、他の手法を用いても良い。

（４）上記実施形態では、移動体制御装置を搭載した移動体として車両Ｃを例示して説明したが、移動体はこれに限らず、ロボットなどの２足歩行をする移動体であっても本発明における移動体制御装置を適用することが可能である。

Ｃ…車両、１ａ、１ｂ…撮影装置、２…画像処理装置、３…３次元計測装置、４…３次元マップ作成装置、５…経路計画装置、６…ＧＰＳ受信機、７…ジャイロ、８…位置・姿勢推定装置、９…運動制御装置、１０…アクチュエータ、２ａ…Ａ／Ｄコンバータ、２ｂ…記憶装置、２ｃ…信号処理装置、２ｄ…ＲＡＭ(Random Access Memory)、ＰＧ…通路検出プログラム、ＧＦ１〜ＧＦｎ…ガボールフィルタ

Claims (12)

1. 移動体の進行方向を撮影する撮影手段と、
   前記撮影手段から得られる撮影画像を基に所定の画像処理を行うことにより、前記撮影画像に存在する通行痕跡を検出する画像処理手段と、
   前記画像処理手段によって検出された通行痕跡を基に前記移動体の移動経路を計画する経路計画手段と、
   を備えることを特徴とする移動経路計画装置。
2. 移動体の進行方向の３次元形状を計測する３次元計測手段と、
   前記３次元計測手段から得られる３次元形状の計測結果を基に地図を作成する地図作成手段と、をさらに備え、
   前記経路計画手段は、前記地図作成手段によって作成された地図及び前記画像処理手段によって検出された通行痕跡を基に前記移動体の移動経路を計画することを特徴とする請求項１記載の移動経路計画装置。
3. 前記画像処理手段は、幅を有する帯状の模様を前記通行痕跡として検出することを特徴とする請求項１または２に記載の移動経路計画装置。
4. 前記画像処理手段は、幅を有し、途中で断続的になっている帯状の模様が複数、３次元空間上で平行になっている部分を前記通行痕跡として検出することを特徴とする請求項１または２に記載の移動経路計画装置。
5. 前記画像処理手段は、幅を有し、途中で断続的になっている帯状の部分を前記撮影画像中から見つけ出し、最も多数の帯状部分が共有する消失点に向かっている帯状部分を前記通行痕跡として絞り込むことを特徴とする請求項１または２に記載の移動経路計画装置。
6. 前記画像処理手段は、
   方向パラメータの異なる複数の空間周波数フィルタを用いて、前記撮影画像における検出対象領域をフィルタリングすることにより、前記検出対象領域についての各方向の強度分布データを生成するフィルタリング処理と、
   前記フィルタリング処理によって生成された前記強度分布データを基に前記検出対象領域における各画素の模様方向を特定する模様方向特定処理と、
   前記模様方向特定処理による前記模様方向の特定結果を基に所定の統計処理を行うことにより、前記検出対象領域において通行痕跡が存在すると推定される痕跡領域を検出する痕跡領域検出処理と、を行う
   ことを特徴とする請求項１または２に記載の移動経路計画装置。
7. 前記画像処理手段は、
   方向パラメータの異なる複数の空間周波数フィルタを用いて、前記撮影画像における検出対象領域をフィルタリングすることにより、前記検出対象領域についての各方向の強度分布データを生成するフィルタリング処理と、
   前記フィルタリング処理によって生成された前記強度分布データを基に前記検出対象領域における各画素の模様方向を特定する模様方向特定処理と、
   前記検出対象領域を奥行き方向に沿って複数に分割して得られる副検出対象領域ごとに、前記模様方向特定処理による前記模様方向の特定結果に基づいて所定の統計処理を行うことにより、前記副検出対象領域において通行痕跡が存在すると推定される痕跡領域を検出する痕跡領域検出処理と、を行う
   ことを特徴とする請求項１または２に記載の移動経路計画装置。
8. 前記画像処理手段は、前記痕跡領域検出処理における前記統計処理として、
   前記検出対象領域における各画素のそれぞれを投票元画素として選択し、前記投票元画素を起点としてその模様方向に平行な直線上に存在する画素、または前記投票元画素を起点としてその模様方向に沿うように設定された投票対象領域に含まれる画素に対して投票を行う投票処理と、
   前記投票処理による投票結果を基に投票回数が最も大きい画素を消失点として特定する消失点特定処理と、
   前記消失点の投票元画素を有効投票画素として抽出する有効投票画素抽出処理と、
   前記検出対象領域において前記消失点を頂点とする領域を複数の小領域に分割し、各小領域の内、前記有効投票画素の比率が所定の閾値を超えた小領域を前記痕跡領域として特定する痕跡領域特定処理と、を行う
   ことを特徴とする請求項６記載の移動経路計画装置。
9. 前記画像処理手段は、前記痕跡領域検出処理における前記統計処理として、
   前記副検出対象領域における各画素のそれぞれを投票元画素として選択し、前記投票元画素を起点としてその模様方向に平行な直線上に存在する画素、または前記投票元画素を起点としてその模様方向に沿うように設定された投票対象領域に含まれる画素に対して投票を行う投票処理と、
   前記投票処理による投票結果を基に投票回数が最も大きい画素を消失点として特定する消失点特定処理と、
   前記消失点の投票元画素を有効投票画素として抽出する有効投票画素抽出処理と、
   前記副検出対象領域において前記消失点を頂点とする領域を複数の小領域に分割し、各小領域の内、前記有効投票画素の比率が所定の閾値を超えた小領域を前記痕跡領域として特定する痕跡領域特定処理と、を行う
   ことを特徴とする請求項７記載の移動経路計画装置。
10. 請求項１〜９のいずれか一項に記載の移動経路計画装置と、
    移動体の動力を発生するアクチュエータと、
    前記移動経路計画装置によって計画された移動経路に沿って移動体が移動するように前記アクチュエータを制御する運動制御手段と、
    を備えることを特徴とする移動体制御装置。
11. 前記移動体の位置に関する情報を取得する位置情報取得手段と、
    前記移動体の姿勢に関する情報を取得する姿勢情報取得手段と、
    前記位置情報取得手段及び前記姿勢情報取得手段によって取得された情報を基に前記移動体の現在位置及び姿勢を推定する位置・姿勢推定手段を備え、
    前記運動制御手段は、前記位置・姿勢推定手段によって推定された移動体の現在位置及び姿勢を基に前記移動体が前記移動経路を中心とする所定の許容範囲内を移動するように前記アクチュエータを制御することを特徴とする請求項１０記載の移動体制御装置。
12. 請求項１０または１１に記載の移動体制御装置を搭載することを特徴とする移動体。